Unmanned aerial vehicle / ship cooperative path tracking control method in variable-speed navigation scene

Abstract

The invention discloses an unmanned aerial vehicle / ship cooperative path tracking control method in a variable-speed navigation scene. The method comprises the following steps: S1, establishing a nonlinear system model of an unmanned aerial vehicle / ship cooperative system; s2, constructing a reference path of a virtual ship and a reference path of a virtual unmanned aerial vehicle; s3, constructing a position error of the nonlinear system model; s4, designing a first virtual control law; s5, designing a self-adaptive sliding mode surface; s6, designing a sliding mode control law and a position adaptive law of the position controller; s7, designing a second virtual control law; s8, designing an attitude control law and an attitude adaptive law of the attitude controller; and S9, realizing variable-speed path tracking cooperative control of the nonlinear system model on the virtual unmanned aerial vehicle / ship. According to the method, the problem that existing sea-air cooperative speed change guidance is vacant is solved, the tracking path of the unmanned aerial vehicle / ship can be efficiently planned according to the traffic condition of the target harbor area water area, and meanwhile, the method has high robustness for system parameter fluctuation and external interference.

Description

Technical Field

[0001] The present invention relates to the technical field of UAV / ship collaborative path tracking, and in particular to a UAV / ship collaborative path tracking control method in a variable speed navigation scenario. Background Art

[0002] In the maritime field, a ship's path-tracking control system consists of three subsystems: guidance, control, and navigation. The guidance system automatically generates a reference signal based on the positional relationship between the UAV / USV system's current attitude and the desired path. The control system achieves effective convergence by stabilizing the error between the current attitude and the reference signal. The navigation system transmits the controlled object's position and attitude information to the guidance and control systems via sensors.

[0003] Guidance and control are two important subsystems in path tracking control. In existing technologies, guidance systems often rely on a designed virtual ship for planning and navigation. The virtual ship generates a reference path based on fixed waypoints and implements a uniform path tracking plan for the ship. However, this approach is still unable to meet certain operational scenarios requiring variable speed navigation. For example, in a ship entering a port, navigation is a variable speed braking process, where the ship enters the port waters at a uniform speed from the port waters outside the port and then decelerates in the port waters. Existing technologies, such as 3D mapping-guided drone / ship path tracking control algorithms, have established a guidance framework for drone / ship collaborative systems. However, their speed control planning and control are not perfect. For example, the lack of variable speed guidance planning prevents high-precision control of drones / ships in variable speed navigation scenarios. Furthermore, traditional adaptive neural network control cannot effectively address the heterogeneous control loop error differences caused by deceleration collaborative guidance, resulting in safety risks and low navigation efficiency during the port entry process. Therefore, this approach is not suitable for ships implementing deceleration port entry tasks with complex traffic flows. Summary of the Invention

[0004] The present invention provides a UAV / ship collaborative path tracking control method in a variable speed navigation scenario to overcome the above technical problems.

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

[0006] A UAV / ship collaborative path tracking control method in a variable speed navigation scenario, comprising the following steps:

[0007] S1: Establish a nonlinear system model of the UAV / ship cooperative system;

[0008] S2: Designing a speed-varying simulated control function and a virtual bow angular velocity, and constructing a reference path of the virtual ship and a reference path of the virtual UAV according to the speed-varying simulated control function and the virtual bow angular velocity;

[0009] S3: constructing a position error of the nonlinear system model according to the reference paths of the virtual ship and the virtual UAV;

[0010] S4: designing a first virtual control law for stabilizing the position error of the nonlinear system model;

[0011] S5: Designing an adaptive sliding surface for the position controller in the nonlinear system model;

[0012] S6: performing adaptive control design based on the adaptive sliding mode surface to obtain a sliding mode control law and a position adaptive law of the position controller in the nonlinear system model;

[0013] S7: defining a posture error of the nonlinear system model based on a sliding mode control law, and designing a second virtual control law for stabilizing the posture error of the nonlinear system model;

[0014] S8: Designing a posture control law and a posture adaptation law of the posture controller in the nonlinear system model;

[0015] S9: According to the designed first virtual control law, second virtual control law, sliding mode control law and position adaptive law of the position controller, and attitude control law and attitude adaptive law of the attitude controller, the nonlinear system model is implemented for the variable speed path tracking collaborative control of the virtual UAV / ship.

[0016] Furthermore, in S1, the nonlinear system model of the UAV / ship cooperative system is established as follows:

[0017]

[0018] in,

[0019]

[0020] Where η1=[x a ,y a ,z a ] T ,η2=[φ a ,θ a ,ψ a ] T , x a ,y a ,z a They represent the forward, lateral and ascending displacements of the UAV, φ a ,θ a ,ψ a Denote the roll, pitch and yaw angles of the UAV respectively, η3=[x s ,y s ,ψs ] T , x s ,y s ,ψ s They represent the ship's forward movement, drift displacement and bow angle in the inertial coordinate system respectively; J(η i' ) is the transformation matrix, i'=1,2,3,ν1=[u ax ,u ay ,u az ] T ,ν2=[p a ,q a ,r a ] T ,u ax ,u ay ,u az They represent the forward, lateral and ascending speeds of the UAV in the body coordinate system respectively; p a ,q a ,r a Denote the rolling, pitching and yaw angular velocities of the UAV respectively, ν3=[u s ,v s ,r s ] T ,u s ,v s ,r s Respectively represent the ship's forward speed, drift speed and bow angular velocity; M i' Both represent the additional mass matrix, M1=diag{m a ,m a ,m a}, M2=diag{I xx ,I yy ,I zz}, M3=diag{m u ,m v ,m r}; where m a Indicates the mass of the drone, I xx ,I yy ,I zz Respectively represent the rotational inertia of the drone along the ox, oy, and oz axes, m u ,m v ,m r Respectively represent the additional mass of the ship model; C(ν1), C(ν2) are zero matrices, C(ν3)=[m v v s r s ,-m u u s r s ,(m u -m v )us v s ] T ; f(ν1), f(ν2) represent the nonlinear term matrices on the position loop and attitude loop of the UAV respectively, and f(ν3) represents the nonlinear term matrix of the ship; where k dx ,k dy ,k dz Denotes a positive parameter constant, d ui' ,d vi' ,d ri' Represents the hydrodynamic damping coefficient of the ship, R i' is the gain matrix of the UAV and the ship, where R2 = diag{1,d,d}, R3 = diag{1,1,1}; d is the diagonal diameter of the UAV, τ(ν i' ) is the control input of the ship and the UAV, τ(ν1)=[τ f ,τ f ,τ f ,] T ,τ(ν2)=[τ φ ,τ θ ,τ ψ ,] T ,τ(ν3)=[τ u ,0,τ r ,] T ; where τ f represents the rotor force of the UAV rotor, τ φ ,τ θ ,τ ψ are the roll, pitch and yaw moments of the UAV, τ u ,τ r They represent the forward thrust and turning moment of the ship, d w (ν1)=[d wx ,d wy ,d wz ] T , d w (ν2)=[d wφ ,d wθ ,d wψ ] T , d w (ν3)=[d wu ,d wv ,d wr ] T , d wx ,d wy ,d wz They represent the external interference forces on the UAV in the forward, drift, and heave directions respectively; d wφ ,d wθ ,d wψRespectively represent the external interference torques on the UAV in the roll, pitch and yaw directions; d wu ,d wv ,d wr They represent the external interference forces on the ship in the forward, drift and bow directions respectively. D1(g) and D2(g) represent the gravity matrix of the UAV, and D3(g) represents the gravity matrix of the ship, where D1(g) = [0,0,m a g] T , D2(g) and D3(g) are both zero matrices; s(·), c(·) and t(·) represent the trigonometric functions sin(·), cos(·) and tan(·), respectively.

[0021] Furthermore, in S2, designing a speed-varying simulated control function and a virtual pitching angular velocity, and constructing a reference path of the virtual ship and a reference path of the virtual UAV according to the speed-varying simulated control function and the virtual pitching angular velocity includes:

[0022] The designed variable speed pseudo-control function u sd As shown in formula (3):

[0023]

[0024] Where u sd0 and t ξ Respectively represent the initial preset speed and speed change time threshold, Respectively represent positive parameter constant, state stability, speed coefficient and positive constant;

[0025] The reference path of the constructed virtual ship is expressed as:

[0026] η 3d =J(η 3d )·ν 3d (4)

[0027] in,

[0028]

[0029] Where η 3d =[x sd ,y sd ,ψ sd ] T , x sd ,y sd ,ψ sd They represent the expected forward and drift distances and the expected heading angle of the ship, ν 3d =[u sd ,0,r sd ] T ; r sd represents the virtual yaw angular velocity;

[0030] The reference path of the constructed virtual drone is expressed as:

[0031]

[0032] Where, Represents the derivative of the virtual drone's forward and drift distances and heading angles.

[0033] Furthermore, in S3, constructing the position error of the nonlinear system model according to the reference paths of the virtual ship and the virtual UAV includes:

[0034] According to the coordinated variable speed guidance algorithm, the ship position error z is defined as se , UAV forward distance error x ae , UAV drift distance error y ae and the UAV heave distance error z ae They are:

[0035]

[0036] x ae =x a -x ad

[0037] y ae =y a -y ad

[0038] z ae =z a -z ad

[0039] Where z a Indicates the lifting displacement of the UAV; z ad Indicates the rise and fall distance of the virtual drone; x se ,y se are the forward and drift distance errors of the ship, respectively, expressed as:

[0040] x se =x s -x sd

[0041] y se =y s -y sd

[0042] The derivative form of the position error of the nonlinear system model is expressed as formula (7):

[0043]

[0044] Where, is the derivative of the UAV’s position error; is the derivative of the desired position information of the UAV; ψ se is the ship's pitch angle error, ψ se =ψ s -ψ sl ;x s ,y s ,ψ s They represent the ship's forward movement, drift displacement and bow angle in the inertial coordinate system respectively; ψ sl is the azimuth from the real ship to the virtual ship, as shown in formula (8):

[0045] ψ sl =0.5[1-sgn(x se )]sgn(y se )π+arctan(y se / x se ) (8).

[0046] Furthermore, in S4, the first virtual control law designed to stabilize the position error of the nonlinear system model includes:

[0047]

[0048] Where, α us ,α uai are the first virtual control rates of the ship and the UAV respectively; k sz and k ai The first virtual control law α is us ,α uai The positive design parameter, δ Δ It is a positive small quantity;

[0049] At the first virtual control rate α us In the design, a saturation variable ψ sat To limit ψ se , as shown in formula (10):

[0050]

[0051] Where, is a positive small quantity;

[0052] Therefore, the first virtual control rate α us Redefine as formula (11):

[0053]

[0054] Introducing dynamic surface control theory to design first-order filter β ι, the derivative of the first virtual control rate is reduced to the following order:

[0055]

[0056] Where,∈ ι is a positive time constant.

[0057] Furthermore, in S5, designing an adaptive sliding surface for the position controller in the nonlinear system model includes:

[0058]

[0059] Where k s ,k i is a positive design parameter;

[0060] And find its derivative, as shown in formula (14):

[0061]

[0062] Where, ω u ,ω i represents the adaptive fuzzy logic function, represents the fuzzy basis function; ε u ,ε uai represents the random approximation error.

[0063] Furthermore, in S6, the specific steps of performing adaptive control design based on the adaptive sliding mode surface to obtain the sliding mode control law and position adaptive law of the position controller in the nonlinear system model include:

[0064] The speed errors of the ship and the UAV are defined as:

[0065]

[0066] Find their derivatives, expressed as:

[0067]

[0068] Where, f us ,f uai Represents the nonlinear terms of the ship's forward movement, the UAV's forward movement, drift, and heave speed models,

[0069] The fuzzy logic system FLS is used to perform online approximation on the above nonlinear terms as shown in formula (17), which is expressed as:

[0070]

[0071] Where, ω u ,ω irepresents the adaptive fuzzy logic function, i=x,y,z, represents the fuzzy basis function, ε u ,ε uai represents the random approximation error;

[0072] Combined with formula (14) in the adaptive sliding surface, we introduce As ω i The estimated value of is used to design the sliding mode control law and position adaptive law of the position controller in the nonlinear system model using adaptive technology and Backstepping technology, as shown in Equations (18) and (19):

[0073]

[0074] Where λ u , λ i , is a positive adaptive parameter.

[0075] Furthermore, in S7, the posture error of the nonlinear system model is defined based on the sliding mode control law, and the specific steps of designing a second virtual control law for stabilizing the posture error of the nonlinear system model include:

[0076] Define the azimuth angle ψ from the drone to the virtual drone al As shown in formula (20):

[0077] ψ al =0.5[1-sgn(x ae )]sgn(y ae )π+arctan(y ae / x ae ) (20)

[0078] Where x ae is the UAV’s forward distance error; y ae is the drift distance error of the UAV;

[0079] The azimuth angle and sliding mode control law of the UAV to the virtual UAV are solved using nonlinear decoupling technology to obtain the reference roll angle and pitch angle φ of the UAV. ad ,θ ad , as shown in formula (21):

[0080]

[0081] Define attitude error, which includes the ship's bow angle error ψ se 、UAV roll error φ ae , pitch error θ ae and the heading angle error ψae , and take their derivatives respectively, we get:

[0082]

[0083] In order to stabilize the attitude error ψ se ,φ ae ,θ ae ,ψ ae , design the corresponding second virtual control law α rs ,α pa ,α qa ,α ra , as shown in formula (23):

[0084]

[0085] Where k sψ ,k aψ ,k aφ ,k aθ is a positive design parameter;

[0086] Introducing dynamic surface control technology to design first-order filter β ζ ,Right now:

[0087]

[0088] Where,∈ ζ is a positive time constant.

[0089] Furthermore, in S8, the specific steps of designing the attitude control law and attitude adaptation law of the attitude controller in the nonlinear system model include:

[0090] The ship's bow angular velocity error, the UAV's bow, roll, and pitch angular velocity errors are defined as:

[0091]

[0092] And taking the derivatives of them, we get:

[0093]

[0094] Where, f rs ,f ξa Represent the nonlinear terms of the ship's bow pitch, UAV's bow pitch, roll, and pitch angular velocity models respectively; i = z, x, y; j = ψ, φ, θ; ξ = r, p, q;

[0095] The fuzzy logic system FLS is used to perform online approximation on the above nonlinear terms, as shown in formula (27):

[0096]

[0097] Where, ω r ,ω j represents the adaptive fuzzy logic function; ε r ,ε j represents the random approximation error;

[0098] Introduction As ω r ,ω j The estimated value of is combined with equations (26) and (27) to design the attitude control law and attitude adaptation law of the attitude controller, as shown in equations (28) and (29):

[0099]

[0100] Where k sr ,k aξ is a positive design parameter; r ,λ j , is a positive adaptive parameter.

[0101] Beneficial effects: Taking into account the engineering requirements of the UAV / ship collaborative system entering the port for navigation, the present invention proposes a UAV / ship collaborative path tracking control method in a variable speed navigation scenario for a deceleration scenario, including: designing a variable speed pseudo-control function and a virtual bow angular velocity, constructing a reference path of the virtual ship and a reference path of the virtual UAV according to the variable speed pseudo-control function and the virtual bow angular velocity, and enabling the collaborative system to smoothly connect the uniform-variable speed navigation task from the waters outside the port to the port area through the variable speed pseudo-control function; constructing the position error of the nonlinear system model according to the reference paths of the virtual ship and the virtual UAV; designing an adaptive sliding surface for the position controller in the nonlinear system model; performing adaptive control design based on the adaptive sliding surface, and obtaining the sliding mode control law and position adaptive law of the position controller in the nonlinear system model, which solves the problem of heterogeneous control loop error differentiation generated by traditional adaptive neural network control for deceleration collaborative guidance; designing the attitude control law and attitude adaptive law of the attitude controller in the nonlinear system model; and realizing the variable speed path tracking collaborative control of the nonlinear system model for the virtual UAV / ship according to the designed sliding mode control law and position adaptive law of the position controller and the attitude control law and attitude adaptive law of the attitude controller. This invention addresses the existing lack of coordinated sea-air variable-speed guidance. It can efficiently plan the tracking path of drones / ships based on traffic conditions in the target port area, effectively addressing the virtual computing load and model structure uncertainty issues in the control system. Furthermore, the invention is highly robust to system parameter fluctuations and external interference. BRIEF DESCRIPTION OF THE DRAWINGS

[0102] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following is a brief introduction to 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 any creative labor.

[0103] Figure 1 This is a flow chart of a UAV / ship collaborative path tracking control method in a variable speed navigation scenario according to the present invention;

[0104] Figure 2 A three-dimensional conceptual diagram of the variable speed guidance information of a UAV / ship in an embodiment of the present invention;

[0105] Figure 3 A two-dimensional conceptual diagram of the variable speed guidance information of a UAV / ship in an embodiment of the present invention;

[0106] Figure 4 (a) is a three-dimensional trajectory diagram of the UAV / ship collaborative port entry path tracking in an embodiment of the present invention; (b) is a planar trajectory diagram of the UAV / ship collaborative port entry path tracking;

[0107] Figure 5 This is a graph showing the change in forward speed of a drone / ship and speed control input data in an embodiment of the present invention;

[0108] Figure 6 This is a comparison diagram of the ship speed state error and speed control input in an embodiment of the present invention. DETAILED DESCRIPTION

[0109] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0110] This embodiment provides a UAV / ship collaborative path tracking control method in a variable speed navigation scenario, such as Figure 1 As shown, the specific steps include:

[0111] S1: Establish a nonlinear system model of the UAV / ship cooperative system;

[0112] In a specific embodiment, in S1, the nonlinear system model of the UAV / ship cooperative system established by combining the matrix structure theory is expressed as formula (1):

[0113]

[0114] in,

[0115]

[0116] Where η1=[x a ,y a ,z a ] T ,η2=[φ a ,θ a ,ψ a ] T , x a ,y a ,z a They represent the forward, lateral and ascending displacements of the UAV, φ a ,θ a ,ψ a Denote the roll, pitch and yaw angles of the UAV respectively, η3=[x s ,y s ,ψ s ] T , x s ,y s ,ψ s They represent the ship's forward movement, drift displacement and bow angle in the inertial coordinate system respectively; J(η i' ) is the transformation matrix, i'=1,2,3,ν1=[u ax ,u ay ,u az ] T ,ν2=[p a ,q a ,r a ] T ,u ax ,u ay ,u az They represent the forward, lateral and ascending speeds of the UAV in the body coordinate system respectively; p a ,q a ,r a Denote the rolling, pitching and yaw angular velocities of the UAV respectively, ν3=[u s ,v s ,r s ] T ,u s ,v s ,r s Respectively represent the ship's forward speed, drift speed and bow angular velocity; M i' Both represent the additional mass matrix, M1=diag{m a ,m a ,m a}, M2=diag{I xx,I yy ,I zz}, M3=diag{m u ,m v ,m r}; where m a Indicates the mass of the drone, I xx ,I yy ,I zz Respectively represent the rotational inertia of the drone along the ox, oy, and oz axes, m u ,m v ,m r Respectively represent the additional mass of the ship model; C(ν1), C(ν2) are zero matrices, C(ν3)=[m v v s r s ,-m u u s r s ,(m u -m v )u s v s ] T ; f(ν1), f(ν2) represent the nonlinear term matrices on the position loop and attitude loop of the UAV respectively, and f(ν3) represents the nonlinear term matrix of the ship; where k dx ,k dy ,k dz Denotes a positive parameter constant, d ui' ,d vi' ,d ri' Represents the hydrodynamic damping coefficient of the ship, R i' is the gain matrix of the UAV and the ship, where R2 = diag{1,d,d}, R3 = diag{1,1,1}; d is the diagonal diameter of the UAV, τ(ν i' ) is the control input of the ship and the UAV, τ(ν1)=[τ f ,τ f ,τ f ,] T ,τ(ν2)=[τ φ ,τ θ ,τ ψ ,] T ,τ(ν3)=[τ u ,0,τ r ,] T ; where τ f represents the rotor force of the UAV rotor, τ φ ,τ θ ,τ ψ are the roll, pitch and yaw moments of the UAV, τ u ,τ rThey represent the forward thrust and turning moment of the ship, d w (ν1)=[d wx ,d wy ,d wz ] T , d w (ν2)=[d wφ ,d wθ ,d wψ ] T , d w (ν3)=[d wu ,d wv ,d wr ] T , d wx ,d wy ,d wz They represent the external interference forces on the UAV in the forward, drift, and heave directions respectively; d wφ ,d wθ ,d wψ Respectively represent the external interference torques on the UAV in the roll, pitch and yaw directions; d wu ,d wv ,d wr They represent the external interference forces on the ship in the forward, drift and bow directions respectively. D1(g) and D2(g) represent the gravity matrix of the UAV, and D3(g) represents the gravity matrix of the ship, where D1(g) = [0,0,m a g] T , D2(g) and D3(g) are both zero matrices; s(·), c(·) and t(·) represent the trigonometric functions sin(·), cos(·) and tan(·), respectively.

[0117] In this embodiment, in order to facilitate the subsequent control design, the gravity matrix D i' The elements in (g) are combined into the nonlinear term f(ν i' )middle.

[0118] Specifically, in this embodiment, any drone / ship not specifically stated to be a virtual drone / ship is a real drone / ship.

[0119] S2: Designing a speed-varying simulated control function and a virtual bow angular velocity, and constructing a reference path of the virtual ship and a reference path of the virtual UAV according to the speed-varying simulated control function and the virtual bow angular velocity;

[0120] In a specific embodiment, in S2, the design Figure 3 The variable speed pseudo-control function u in sd As shown in formula (3):

[0121]

[0122] Where u sd0 and t ξ Respectively represent the initial preset speed and speed change time threshold, They represent positive parameter constants, state stability variables, speed coefficients and positive constants respectively.

[0123] Specifically, the virtual guidance path generated by the coordinated variable speed guidance proposed in this embodiment requires the user to set the virtual bow angular velocity r according to the real-time traffic environment of the port area. sd and the speed control function u sd To generate the reference path. In the guidance design process, r sd In the pre-steering period, a smaller change amplitude value can be given to achieve the best effect of combining the actual navigation conditions and the best navigation route as much as possible. sd The lateral load and torque on the hull can be reduced, thereby improving the navigation capability of the collaborative system and, to a certain extent, avoiding the overshoot phenomenon caused by the nonlinear system model on the virtual path.

[0124] This embodiment uses a variable speed simulated control function to perform real-time system speed control, and combines the fuzzy logic system FLS (Fuzzy Logic System) to deal with the model uncertainty problem in the UAV / ship system, which can realize the feasibility of the UAV / ship system in the sea-air collaborative port entry mission. Specifically, the variable speed simulated control function serves as the virtual expected speed of the collaborative system. It can guide the UAV / ship to achieve a prerequisite navigation condition of deceleration by triggering the speed change time threshold when entering the port area, so that the collaborative system can smoothly connect the uniform-variable speed navigation task from the waters outside the port to the port area. At the same time, the variable speed simulated control function can customize the virtual ship's steering angular velocity for guidance according to the real-time traffic environment, and then dynamically plan the reference signals of both the UAV search performance (navigation speed, sea surface scanning capability) and the ship's navigation status.

[0125] like Figure 4 As shown in (a) and (b), the reference path of the constructed virtual ship is expressed as:

[0126] η 3d =J(η 3d )·ν 3d (4)

[0127] in,

[0128]

[0129] Where η 3d =[x sd ,y sd ,ψ sd ] T , xsd ,y sd ,ψ sd They represent the expected forward movement, drift distance and expected heading angle of the ship, ν 3d =[u sd ,0,r sd ] T ; r sd represents the virtual yaw angular velocity;

[0130] The reference path of the constructed virtual drone is expressed as:

[0131]

[0132] Where, Represents the derivative of the virtual drone's forward and drift distances and heading angles.

[0133] S3: constructing a position error of the nonlinear system model according to the reference paths of the virtual ship and the virtual UAV;

[0134] In a specific embodiment, in S3, constructing the position error of the nonlinear system model according to the reference paths of the virtual ship and the virtual drone includes:

[0135] According to the coordinated variable speed guidance algorithm, the ship position error z is defined as se , UAV forward distance error x ae , UAV drift distance error y ae and the UAV heave distance error z ae They are:

[0136]

[0137] x ae =x a -x ad

[0138] y ae =y a -y ad

[0139] z ae =z a -z ad

[0140] Where z ad Indicates the rise and fall distance of the virtual drone, and the specific value is set by the user; z ad Indicates the rise and fall distance of the virtual drone; x se ,y se are the forward and drift distance errors of the ship, respectively, expressed as:

[0141] x se=x s -x sd

[0142] y se =y s -y sd

[0143] The derivative form of the position error of the nonlinear system model is expressed as formula (7):

[0144]

[0145] Where, is the derivative of the UAV’s position error; is the derivative of the desired position information of the UAV; ψ se is the ship's pitch angle error, ψ se =ψ s -ψ sl ;x s ,y s ,ψ s They represent the ship's forward movement, drift displacement and bow angle in the inertial coordinate system respectively; ψ sl is the azimuth from the real ship to the virtual ship, as shown in formula (8):

[0146] ψ sl =0.5[1-sgn(x se )]sgn(y se )π+arctan(y se / x se ) (8).

[0147] S4: designing a first virtual control law for stabilizing the position error of the nonlinear system model;

[0148] In a specific embodiment, in S4, in order to stabilize the position error z se ,x ae ,y ae ,z ae , the first virtual control law designed to stabilize the position error of the nonlinear system model is as follows:

[0149]

[0150] Where, α us ,α uai are the first virtual control rates of the ship and the UAV respectively; k sz and k ai The first virtual control law α is us ,α uai The positive design parameter, δ Δ It is a positive small quantity;

[0151] At the first virtual control rate α us In the design, considering cos(ψ se ) as the denominator, a saturation variable ψ sat To limit ψ se , so that |ψ se |<0.5π, as shown in formula (10):

[0152]

[0153] Where, is a positive small quantity;

[0154] Therefore, the first virtual control rate α us Redefine as formula (11):

[0155]

[0156] The first virtual control rate will cause a large computational load in the subsequent derivation, leading to the problem of complexity explosion. Therefore, the dynamic surface control theory is introduced to design the first-order filter β ι , the derivative of the first virtual control rate is reduced to the order, that is,

[0157]

[0158] Where,∈ ι is a positive time constant, and the filter error q ι =β ι -α ι ;

[0159] S5: Designing an adaptive sliding surface for the position controller in the nonlinear system model;

[0160] In a specific embodiment, in S5, the adaptive sliding mode surface designed for the position controller in the nonlinear system model is as shown in formula (13):

[0161]

[0162] Where k s ,k i is a positive design parameter;

[0163] And find its derivative, as shown in formula (14):

[0164]

[0165] S6: performing adaptive control design based on the adaptive sliding mode surface to obtain a sliding mode control law and a position adaptive law of the position controller in the nonlinear system model;

[0166] In a specific embodiment, in S6, performing adaptive control design based on the adaptive sliding mode surface to obtain the sliding mode control law and position adaptive law of the position controller in the nonlinear system model includes:

[0167] The speed errors of the ship and the UAV are defined as:

[0168]

[0169] Get their derivatives, expressed as:

[0170]

[0171] Where, f us ,f uai represents the nonlinear terms of the ship's forward movement, the UAV's forward movement, drift, and heave speed models. This embodiment uses FLS to perform online approximation on the above nonlinear terms as shown in Equation (17), which is expressed as:

[0172]

[0173] Where, ω u ,ω i represents the adaptive fuzzy logic function, i=x,y,z, represents the fuzzy basis function, ε u ,ε uai represents the random approximation error;

[0174] Combined with formula (14) in the adaptive sliding surface, we introduce As ω i The estimated value of is used to design the sliding mode control law and position adaptive law of the position controller in the nonlinear system model using adaptive technology and Backstepping technology, as shown in Equations (18) and (19):

[0175]

[0176] Where λ u , λ i , is a positive adaptive parameter, i=x,y,z.

[0177] Specifically, this embodiment introduces adaptive sliding mode control (SMC) for the position controller to solve the problem of virtual computing load in the control system, and combines fuzzy logic system (FLS) and adaptive dynamic surface technology to reduce tracking error and improve path tracking accuracy while ensuring control performance, so as to realize the task of coordinated drone / ship entering the port.

[0178] S7: defining a posture error of the nonlinear system model based on a sliding mode control law, and designing a second virtual control law for stabilizing the posture error of the nonlinear system model;

[0179] In a specific embodiment, in S7, the specific steps of defining the posture error of the nonlinear system model based on the sliding mode control law and designing a second virtual control law for stabilizing the posture error of the nonlinear system model include:

[0180] like Figure 2 As shown, the azimuth angle ψ from the UAV to the virtual UAV is defined as al As shown in formula (20):

[0181] ψ al =0.5[1-sgn(x ae )]sgn(y ae )π+arctan(y ae / x ae ) (20)

[0182] Where x ae is the UAV’s forward distance error; y ae is the drift distance error of the UAV;

[0183] The azimuth angle and sliding mode control law of the UAV to the virtual UAV are solved using nonlinear decoupling technology to obtain the reference roll angle and pitch angle φ of the UAV. ad ,θ ad , as shown in formula (21):

[0184]

[0185] In order to control the current attitude of the UAV / ship to converge to the reference attitude, the attitude error is defined. The attitude error includes the ship's bow angle error ψ se 、UAV roll error φ ae , pitch error θ ae and the heading angle error ψ ae , and take their derivatives respectively, we get:

[0186]

[0187] In order to stabilize the attitude error ψ se ,φ ae ,θ ae ,ψ ae , design the corresponding second virtual control law α rs ,α pa ,α qa ,α ra , as shown in formula (23):

[0188]

[0189] Where k sψ ,k aψ ,k aφ ,k aθ is a positive design parameter;

[0190] In order to avoid the complexity explosion problem caused by the second virtual control rate (23) in the subsequent derivative calculation, the dynamic surface control technology is introduced to design the first-order filter β j ,Right now:

[0191]

[0192] Where,∈ ζ is a positive time constant, and the filter error q ζ =β ζ -α ζ ;

[0193] S8: Designing a posture control law and a posture adaptation law of the posture controller in the nonlinear system model;

[0194] In a specific embodiment, in S8, the specific steps of designing the attitude control law and attitude adaptation law of the attitude controller in the nonlinear system model include:

[0195] The ship's bow angular velocity error, the UAV's bow, roll, and pitch angular velocity errors are defined as:

[0196]

[0197] And taking the derivatives of them, we get:

[0198]

[0199] Where, f rs ,f ξa They represent the nonlinear terms of the ship's bow, UAV's bow, roll, and pitch angular velocity models, respectively. In this embodiment, FLS is used to perform online approximation on the above nonlinear terms, as shown in formula (27):

[0200]

[0201] Where, ω r ,ω j represents the adaptive fuzzy logic function; ε r ,ε j represents the random approximation error;

[0202] Introduction As ω r ,ω j The estimated value of is combined with equations (26) and (27) to design the attitude control law and attitude adaptation law of the attitude controller, as shown in equations (28) and (29):

[0203]

[0204] Where k sr ,k aξ is a positive design parameter; r ,λ j , is a positive adaptive parameter.

[0205] S7: Implementing the variable speed path tracking collaborative control of the virtual UAV / ship by the nonlinear system model according to the designed first virtual control law, the second virtual control law, the sliding mode control law and the position adaptive law of the position controller, and the attitude control law and the attitude adaptive law of the attitude controller.

[0206] In this embodiment, the effectiveness and superiority of the algorithm proposed in this invention are verified by simulating the sea-air coordinated port approach under external environmental interference. In order to perform the sea-air coordinated deceleration port approach task, the parameters of the speed control function are designed. The expected angular velocity r of the nonlinear system model of the UAV / ship cooperative system sd The design is as shown in formula (30):

[0207]

[0208] The coordinates of the starting point of the port entry mission are set to [-10m, 10m], and a uniform speed guidance is performed on the path before the port. The initial state of the nonlinear system model of the UAV / ship collaborative system is

[0209] [x s (1),y s (1),z s (1),ψ s (1),u s (1),v s (1),r s (1),x a (1),y a (1),z a(1),φ a (1),θ a (1),ψ a (1),u ax (1),u ay (1),u az (1),p a (1),q a (1),r a (1)]=[-10m,10m,0m,0deg,4m / s,0m / s,0rad / s,-6m,6m,0m,0deg,0deg,0deg,4m / s,0m / s,0m / s,0rad / s,0rad / s,0rad / s,].

[0210] Figure 4 and Figure 5 The figure shows the simulation results of UAV / ship collaborative port entry simulated on the MATLAB simulation platform. Figure 4 (a) and Figure 4 As can be seen in (b), the reference path of the UAV / ship is obtained according to the coordinated variable speed guidance planning, which can achieve high-precision tracking control effect in the target port waters. Figure 5 As shown in Figure 3, by introducing the adaptive sliding mode control algorithm to optimize the related speed control, the ship's speed can fluctuate more steadily around the desired speed within a certain range, and the speed input of its system controller becomes smoother.

[0211] In order to further verify the effectiveness and superiority of the algorithm proposed in this embodiment, the speed change time threshold t ξ and the total running time step t m For 120s and 340s, the algorithm proposed in this embodiment is compared with the adaptive neural control algorithm in the prior art. Figure 6 As shown in the figure, the enlarged local details show the superiority of the algorithm proposed in this embodiment, where the blue solid line is the algorithm proposed in this embodiment, and the red dotted line is the comparison algorithm. It can be clearly seen that the absolute values of the relevant ship position and speed errors and the change threshold of the speed control input obtained by the algorithm proposed in this embodiment are smaller than those of the comparison algorithm, indicating that the algorithm proposed in this embodiment has obvious benefits in improving the tracking accuracy of the collaborative system and reducing the state error.

[0212] 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 above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above 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 UAV / ship collaborative path tracking control method in a variable speed navigation scenario, characterized in that: The specific steps include: S1: Establish a nonlinear system model of the UAV / ship cooperative system; S2: Designing a speed-varying simulated control function and a virtual bow angular velocity, and constructing a reference path of the virtual ship and a reference path of the virtual UAV according to the speed-varying simulated control function and the virtual bow angular velocity; S3: constructing a position error of the nonlinear system model according to the reference paths of the virtual ship and the virtual UAV; S4: designing a first virtual control law for stabilizing the position error of the nonlinear system model; S5: Designing an adaptive sliding surface for the position controller in the nonlinear system model; S6: performing adaptive control design based on the adaptive sliding mode surface to obtain a sliding mode control law and a position adaptive law of the position controller in the nonlinear system model; S7: defining a posture error of the nonlinear system model based on a sliding mode control law, and designing a second virtual control law for stabilizing the posture error of the nonlinear system model; S8: Designing a posture control law and a posture adaptation law of the posture controller in the nonlinear system model; S9: According to the designed first virtual control law, second virtual control law, sliding mode control law and position adaptive law of the position controller, and attitude control law and attitude adaptive law of the attitude controller, the nonlinear system model is implemented for the variable speed path tracking collaborative control of the virtual UAV / ship.

2. The UAV / ship collaborative path tracking control method in a variable speed navigation scenario according to claim 1 is characterized in that: In S1, the nonlinear system model of the UAV / ship cooperative system is established as follows: in, Where η1=[x a ,y a ,z a ] T ,η2=[φ a ,θ a ,ψ a ] T , x a ,y a ,z a They represent the forward, lateral and ascending displacements of the UAV, φ a ,θ a ,ψ a Denote the roll, pitch and yaw angles of the UAV respectively, η3=[x s ,y s ,ψ s ] T , x s ,y s ,ψ s They represent the ship's forward movement, drift displacement and bow angle in the inertial coordinate system respectively; J(η i' ) is the transformation matrix, i'=1,2,3,ν1=[u ax ,u ay ,u az ] T ,ν2=[p a ,q a ,r a ] T ,u ax ,u ay ,u az They represent the forward, lateral and ascending speeds of the UAV in the body coordinate system respectively; p a ,q a ,r a Denote the rolling, pitching and yaw angular velocities of the UAV respectively, ν3=[u s ,v s ,r s ] T ,u s ,v s ,r s Respectively represent the ship's forward speed, drift speed and bow angular velocity; M i' Both represent the additional mass matrix, M1=diag{m a ,m a ,m a }, M2=diag{I xx ,I yy ,I zz }, M3=diag{m u ,m v ,m r }; where m a Indicates the mass of the drone, I xx ,I yy ,I zz Respectively represent the rotational inertia of the drone along the ox, oy, and oz axes, m u ,m v ,m r Respectively represent the additional mass of the ship model; C(ν1), C(ν2) are zero matrices, C(ν3)=[m v v s r s ,-m u u s r s ,(m u -m v )u s v s ] T ; f(ν1), f(ν2) represent the nonlinear term matrices on the position loop and attitude loop of the UAV respectively, and f(ν3) represents the nonlinear term matrix of the ship; where k dx ,k dy ,k dz Denotes a positive parameter constant, d ui' ,d vi' ,d ri' Represents the hydrodynamic damping coefficient of the ship, R i' is the gain matrix of the UAV and the ship, where R2 = diag{1,d,d}, R3 = diag{1,1,1}; d is the diagonal diameter of the UAV, τ(ν i' ) is the control input of the ship and the UAV, τ(ν1)=[τ f ,τ f ,τ f ,] T ,τ(ν2)=[τ φ ,τ θ ,τ ψ ,] T ,τ(ν3)=[τ u ,0,τ r ,] T ; where τ f represents the rotor force of the UAV rotor, τ φ ,τ θ ,τ ψ are the roll, pitch and yaw moments of the UAV, τ u ,τ r They represent the forward thrust and turning moment of the ship, d w (ν1)=[d wx ,d wy ,d wz ] T , d w (ν2)=[d wφ ,d wθ ,d wψ ] T , d w (ν3)=[d wu ,d wv ,d wr ] T , d wx ,d wy ,d wz They represent the external interference forces on the UAV in the forward, drift, and heave directions respectively; d wφ ,d wθ ,d wψ Respectively represent the external interference torques on the UAV in the roll, pitch and yaw directions; d wu ,d wv ,d wr They represent the external interference forces on the ship in the forward, drift and bow directions respectively. D1(g) and D2(g) represent the gravity matrix of the UAV, and D3(g) represents the gravity matrix of the ship, where D1(g) = [0,0,m a g] T , D2(g) and D3(g) are both zero matrices; s(·), c(·) and t(·) represent the trigonometric functions sin(·), cos(·) and tan(·), respectively.

3. The UAV / ship collaborative path tracking control method in a variable speed navigation scenario according to claim 2 is characterized in that: In S2, a speed-varying simulated control function and a virtual pitching angular velocity are designed, and a reference path of a virtual ship and a reference path of a virtual UAV are constructed according to the speed-varying simulated control function and the virtual pitching angular velocity, including: The designed variable speed pseudo-control function u sd As shown in formula (3): Where u sd0 and t ξ Respectively represent the initial preset speed and speed change time threshold, Respectively represent positive parameter constant, state stability, speed coefficient and positive constant; The reference path of the constructed virtual ship is expressed as: or 3d =J(η 3d )·n 3d (4) in, Where η 3d =[x sd ,y sd ,ψ sd ] T , x sd ,y sd ,ψ sd They represent the expected forward and drift distances and the expected heading angle of the ship, ν 3d =[u sd ,0,r sd ] T ; r sd represents the virtual yaw angular velocity; The reference path of the constructed virtual drone is expressed as: Where, Represents the derivative of the virtual drone's forward and drift distances and heading angles.

4. The UAV / ship collaborative path tracking control method in a variable speed navigation scenario according to claim 3 is characterized in that: In S3, constructing the position error of the nonlinear system model according to the reference paths of the virtual ship and the virtual UAV includes: According to the coordinated variable speed guidance algorithm, the ship position error z is defined as se , UAV forward distance error x ae , UAV drift distance error y ae and the UAV heave distance error z ae They are: x ae =x a -x ad and ae =and a -and ad With ae =z a -With ad Where z a Indicates the lifting displacement of the UAV; z ad Indicates the rise and fall distance of the virtual drone; x se ,y se are the forward and drift distance errors of the ship, respectively, expressed as: x se =x s -x sd and se =and s -and sd The derivative form of the position error of the nonlinear system model is expressed as formula (7): Where, is the derivative of the UAV’s position error; is the derivative of the desired position information of the UAV; ψ se is the ship's pitch angle error, ψ se =ψ s -ψ sl ;x s ,y s ,ψ s They represent the ship's forward movement, drift displacement and bow angle in the inertial coordinate system respectively; ψ sl is the azimuth from the real ship to the virtual ship, as shown in formula (8): ψ sl =0.5[1-sgn(x se )]sgn(and se )π+arctan(y se / x se ) (8)。 5. The UAV / ship collaborative path tracking control method in a variable speed navigation scenario according to claim 4 is characterized in that: In S4, the first virtual control law designed to stabilize the position error of the nonlinear system model includes: Where, α us ,α uai are the first virtual control rates of the ship and the UAV respectively; k sz and k ai The first virtual control law α is us ,α uai The positive design parameter, δ Δ It is a positive small quantity; At the first virtual control rate α us In the design, a saturation variable ψ sat To limit ψ se , as shown in formula (10): Where l is a small positive quantity; Therefore, the first virtual control rate α us Redefine as formula (11): Introducing dynamic surface control theory to design first-order filter β ι , the derivative of the first virtual control rate is reduced to the following order: Where,∈ ι is a positive time constant.

6. The UAV / ship collaborative path tracking control method in a variable speed navigation scenario according to claim 5 is characterized in that: In S5, designing an adaptive sliding surface for the position controller in the nonlinear system model includes: Where k s ,k i is a positive design parameter; And find its derivative, as shown in formula (14): Where, ω u ,ω i represents the adaptive fuzzy logic function, represents the fuzzy basis function; ε u ,ε uai represents the random approximation error.

7. The UAV / ship collaborative path tracking control method in a variable speed navigation scenario according to claim 6 is characterized in that: In S6, the specific steps of performing adaptive control design based on the adaptive sliding mode surface to obtain the sliding mode control law and position adaptive law of the position controller in the nonlinear system model include: The speed errors of the ship and the UAV are defined as: Find their derivatives, expressed as: Where, f us ,f uai Represents the nonlinear terms of the ship's forward movement, the UAV's forward movement, drift, and heave speed models, The fuzzy logic system FLS is used to perform online approximation on the above nonlinear terms as shown in formula (17), which is expressed as: Where, ω u ,ω i represents the adaptive fuzzy logic function, i=x,y,z, represents the fuzzy basis function, ε u ,ε uai represents the random approximation error; Combined with formula (14) in the adaptive sliding surface, we introduce As ω i The estimated value of is used to design the sliding mode control law and position adaptive law of the position controller in the nonlinear system model using adaptive technology and Backstepping technology, as shown in Equations (18) and (19): Where λ u , λ i , is a positive adaptive parameter.

8. The UAV / ship collaborative path tracking control method in a variable speed navigation scenario according to claim 7 is characterized in that: In S7, the specific steps of defining the attitude error of the nonlinear system model based on the sliding mode control law and designing a second virtual control law for stabilizing the attitude error of the nonlinear system model include: Define the azimuth angle ψ from the drone to the virtual drone al As shown in formula (20): ψ al =0.5[1-sgn(x ae )]sgn(and ae )π+arctan(y ae / x ae ) (20) Where x ae is the UAV’s forward distance error; y ae is the drift distance error of the UAV; The azimuth angle and sliding mode control law of the UAV to the virtual UAV are solved using nonlinear decoupling technology to obtain the reference roll angle and pitch angle φ of the UAV. ad ,θ ad , as shown in formula (21): Define attitude error, which includes the ship's bow angle error ψ se 、UAV roll error φ ae , pitch error θ ae and the heading angle error ψ ae , and take their derivatives respectively, we get: In order to stabilize the attitude error ψ se ,φ ae ,θ ae ,ψ ae , design the corresponding second virtual control law α rs ,α pa ,α qa ,α ra , as shown in formula (23): Where k sψ ,k aψ ,k aφ ,k aθ is a positive design parameter; Introducing dynamic surface control technology to design first-order filter β ζ ,Right now: Where,∈ ζ is a positive time constant.

9. The UAV / ship collaborative path tracking control method in a variable speed navigation scenario according to claim 8, characterized in that: In S8, the specific steps of designing the attitude control law and attitude adaptation law of the attitude controller in the nonlinear system model include: The ship's bow angular velocity error, the UAV's bow, roll, and pitch angular velocity errors are defined as: And taking the derivatives of them, we get: Where, f rs ,f ξa Represent the nonlinear terms of the ship's bow pitch, UAV's bow pitch, roll, and pitch angular velocity models respectively; i = z, x, y; j = ψ, φ, θ; ξ = r, p, q; The fuzzy logic system FLS is used to perform online approximation on the above nonlinear terms, as shown in formula (27): Where, ω r ,ω j represents the adaptive fuzzy logic function; ε r ,ε j represents the random approximation error; Introduction As ω r ,ω j The estimated value of is combined with equations (26) and (27) to design the attitude control law and attitude adaptation law of the attitude controller, as shown in equations (28) and (29): Where k sr ,k aξ is a positive design parameter; r ,λ j , is a positive adaptive parameter.

Images