LU501833B1 - Adaptive cooperative path following control method for usv-uav based on 3d mapping guidance - Google Patents

Abstract

The invention discloses an adaptive cooperative path following control method for USV (Underactuated Surface Vehicle)-UAV (Unmanned Aerial Vehicle) based on 3D mapping guidance, which comprises the following steps that S1, models of a USV-UAV cooperative system are established; S2, effective connection between a USV and a UAV is established; S3, a USV-UAV position controller and adaptive law are designed; S4, a USV-UAV attitude controller and adaptive law are designed; S5, the USV-UAV is controlled to complete cooperative path following control operations; according to the adaptive cooperative path following control method for USV-UAV based on 3D mapping guidance, surface reference path information can be equivalently mapped to an aerial reference surface, effective connection between the USV and the UAV can be established, the USV and UAV system can be controlled at the same time, and the problems of structural uncertainty and explosion of the complexity in the USV-UAV cooperative system can be solved by using a fuzzy logic system and dynamic surface control technique; in this way, the automaticity of the USV-UAV in the aspect of cooperative path following can be improved.

Description

ADAPTIVE COOPERATIVE PATH FOLLOWING CONTROL 0501833 METHOD FOR USV-UAV BASED ON 3D MAPPING GUIDANCE

FIELD OF THE INVENTION The invention relates to the field of robot cooperative control, in particular to an adaptive cooperative path following control method for USV-UAV based on 3D mapping guidance.

BACKGROUND OF THE RELATED ART The ILOS guidance algorithm is widely applied in the fields of ship and unmanned aerial vehicle motion control. The endurance and loading capacity of underactuated surface vehicles (USV) are better than that of unmanned aerial vehicles (UAV), and the UAV has advantages in the velocity and observation capacity. Therefore, compared with a single USV or UAV, when a USV carries a UAV to carry out maritime operations, the flexibility, practicability and expandability can be improved, and air-sea cooperative advantages can be fully exploited.

However, in the current research results, cooperative control of the homogeneous agent and first-order /second-order heterogeneous agent is mainly researched, but the actual engineering conditions of the USV-UAV are not considered; a series of results are achieved in the air-sea cooperative control, but a complete control theory system for USV-UAV cooperative voyage with the remote-control technology is not formed; in addition, the ILOS guidance algorithm plans reference heading signals based on forward visibility range, so the actual path is prone to overshoot at turning points.

Based on the above analysis, during USV-UAV cooperative control operations, the USV or UAV path following control algorithm based on the ILOS guidance mainly has the following defects: the ILOS guidance algorithm is prone to overshoot at the turning points to influence the control performance; the LOS guidance algorithm cannot establish the effective connection between the USV and the UAV, so that the USV and the UAV cannot complete automatic cooperative path following control operations.

SUMMARY OF THE INVENTION The invention provides an adaptive cooperative path following control method for USV- UAV based on 3D mapping guidance to solve the above questions.

The adaptive cooperative path following control method comprises the following steps 1 that: LU501833 S1, models of a USV-UAV cooperative system are established as a controlled object of a to-be-designed controller in the subsequent step; S2, effective connection between a USV and a UAV is established, and position information is equivalently mapped to an aerial reference surface of the UAV through 3D mapping guidance to obtain a reference heading of the USV on the surface and a reference heading of the UAV on the aerial surface; the position information refers to the position information of a desired ship route designed by an logic virtual ship for the USV; S3, a USV-UAV position controller and adaptive law are designed, USV-UAV position errors are reduced, the USV-UAV are guided to follow a reference position, and the position controller is subjected to decoupling by using a nonlinear decoupling technique to obtain a reference roll angle and a reference pitch angle of the UAV; S4, a USV-UAV attitude controller and adaptive law are designed, and USV-UAV attitude errors are reduced; SS, the USV-UAV is controlled to complete cooperative path following control operations.

Furthermore, the models of the USV-UAV cooperative system in S1 are as follows: Xs = U, COS( bg) — V, SIN( by) ys = U, Sin( bg) + V, cos( bg) Xa = Uaxy Ya = Uay, Za = Uaz b, = Pa 0 à = da Va = Ta Ug = 2224 ue dax = =u, + = RyeFr + dy day = — uy + ERyF; + duy (2) lay = — Lu, — g + —R,Fr + du, da = HSE da dE hat To +dwo h= #46 0, — bte Typ + dwv 2 vs) = var, = Lu, dy gy, ey LU501833 f,(vy) = ur — ay, — = [VsIVs — ce? f( vs) = us rs SE rr, — 221] (3) Ry = cos(¢,) sin( 6 ,) cos( b 4) + sin( 9 4) sin( b 4) R, = cos(¢,) sin( 9 ,) cos( b 4) — sin( 9 ,) sin( +) R, = cos(¢,) cos( b ,) wherein, formula (1) and formula (2) are the models of the USV-UAV cooperative system, formula (1) is the kinematic model, formula (2) is the dynamical model, and formula (3) is the expansion formula for some variables in formula (2); [x Vir Zar Par Par V 1 j = s, a represent the surge, drift and heave displacement and the roll, pitch and heading angle of the USV-UAV cooperative system; v, = [Us, Vs, T5] represent the surge, drift and yaw velocity of the USV, and v, = [Uax» Uay» Vaz» Pa Ja ral represent the velocity and rotating angular velocity of the UAV along the front and rear direction, left and right direction and up and down direction; My My, M, represent the additional mass of the models, and d;;, d;,, d;3, | = u, v, 1 represent nonlinear damping terms of the models; J, represents the inertia of rotors of the UAV, Kox» Koy, Koz Tepresent the pneumatic friction coefficients, m represents the mass of the UAV, g represents the gravitational acceleration, I.y,1,y,1,, represent the moment of inertia, k ax, Kay» Kaz represent the rotational resistance coefficients, and A, = 2; — 2, + 23 — (,, 2; 1 = 1,2,3,4 are the angular velocity of the rotors. di i = U, V,T, X, Y, Z, ¢, 6, represents the external disturbance force or moment that the USV-UAV cooperative system is subjected to; Fy represents the resultant force of the force Fj, i = 1,2,3,4 of the rotors, Ty, Tg, Ty represent the roll, pitch and yaw moment of the UAV, and 7,,, T, represent the surge thrust and the turning moment of the USV.

Furthermore, for the effective connection between the USV and the UAV established in 82: the reference path of the USV is as follows: Kg] = Us COS Ws Ys = Ug SIN Ps; Vs = Ts (4) wherein, (xg, Vg, Ws) represent the position coordinates and the heading angle of the logic virtual ship, and ug, rg; represent the surge velocity and the yaw angular velocity of the logic virtual ship.

In order to establish the effective connection between the USV and the UAV, an equivalent mapping technique is used to map the position information of the logic virtual ship to the aerial reference surface of the UAV to provide real-time reference position 3 information for the UAV, that is, Xa1 = Xst Yu = Vs, Wherein, the vertical position za; of the LU501833 UAV is manually set generally; according to the relationship between the current position and reference path of the USV- UAV cooperative system, reference heading signals of the USV-UAV are calculated: Xje = Xj1 7 Xj» Vje = Yj1 7 Yj Yq = 0.5[1 — sgn(xje)] sgn(vje) x + arctan(y;e/Xje).j = s, a (5) wherein, Xje, Yje, j = S, a represent the distances between current position coordinates to desired position coordinates of the USV and the UAV respectively; Wa, j = S, a represent reference heading angles of the USV and the UAV respectively; sgn ( * ) represents a sign function.

Furthermore, the USV-UAV position controller designed in S3 is as follows: Ty = Ksullse + Bus — Dus (ve) + (2 — 84 ) cosh) Fr =m [c2 + c3 + (cz + 9)” Cx = —KaxuYaxe + Buax — OxPalVa) — Xae Cy = —KayuYaye + Buay — @yPa(Va) — Yae cz = ~Kazullaze + Buaz — ®20a(Va) — ZaeTu (6) the USV-UAY adaptive law designed in S3 is as follows: Ou = Yoru thse Ps (Vs) — Tou (By — Ou(0))] Bx = Vor [UaxePa(Va) — Tur (@x — Bx(0))] By = Voy [UayePa(Va) = Foy (@y — B,(0))] Oz = Yoz|Yaze PalVa) — 9wz(@ — @,(0))] (7) wherein, ug, represents the difference value between the velocity and the virtual velocity control law of the USV, Bys represents dynamic surface signals of the virtual velocity control law of the USV, £, represents the linear distance of the USV to the reference position, and yg, represents the heading errors of the USV. c,, C,, Cz represent intermediate control variables that are used to simplify control, Ugye, Ugye, Vaze Tepresent the difference values between the velocity on the x-axis, y-axis and z-axis directions and the virtual velocity control laws of the UAV respectively, Buax» Puay» Puaz Tepresent the dynamic surface signals of the virtual velocity control law respectively, and Xae» Vae Zae represent the distance differences from the current coordinates on the x-axis, y-axis and z-axis to the desired position coordinates 4 respectively.

Ksur Kaxu Fayu» Fazu Tepresent positive controller parameters of the USV-UAV LU501833 on the surge degree of freedom, x-axis, y-axis and z-axis directions respectively, You Yoox Yoy Yoz TEPresent positive design parameters of the adaptive law of the USV-UAV on the surge degree of freedom, x-axis, y-axis and z-axis directions respectively, Owuw Tax Joy» Taz Tepresent positive design parameters of the anti-drift terms of adaptive law of the USV-UAV on the surge degree of freedom, x-axis, y-axis and z-axis directions respectively, Ea(Va) and p,(V;) represent fuzzy basic functions, w;, i = u, x, y, Z represents adaptive parameters of a fuzzy system, and &;,i = u, x, y, Z represents observation values of adaptive parameters of the fuzzy system.

Furthermore, the USV-UAV attitude controller designed in S4 is as follows:

ty = —KorTse + Bre — B05 (vs) — Wie

Ty = byt [-KartTae — DyPalVa) + Pra — Vae]

Te = b5*[-KapPae — Da PalVa) + Ppa — Pac]

Te = by [—Kaglae — De Pa(Ya) + Bga — Pac] (8)

the USV-UAY adaptive law designed in S4 is as follows:

D, = Voor [Te 95s (Vs) — Gor (@r — @(0))]

Dp = Yoy [Fae Pala) — Joy (ay — ©,(0))]

Dp = Yod [Pac Pa (Va) — On (9 — 040)

de = Yoo[JacPa(Ya) — Kwe(@e — Dg (0))] (9)

wherein, r,e represents the difference value between the yaw velocity and the virtual yaw velocity control law of the USV, By represents dynamic surface signals of the virtual yaw velocity control law of the USV, and WP, represents heading errors of the USV. 14., Dyer ae represent difference values between the yaw, roll and pitch velocity on the x-axis, y-axis and z-axis direction and the corresponding virtual velocity control laws of the UAV respectively,

Bra Ppa Pqa represent dynamic surface signals of virtual yaw, roll and pitch velocity control law respectively, and Vae Paer Oe represent difference values between the current heading angle, roll angle and pitch angle and the desired heading angle, roll angle and pitch angle of the UAV respectively; Ksr, Kar, Kap, Kaq represent positive controller parameters of the USV- UAV on the yaw, roll and pitch degrees of freedom respectively, Yur, Yay Yod» Yoo Tepresent positive design parameters of the USV-UAV adaptive control law on the yaw, roll and pitch degrees of freedom respectively, and Gy, Oy, Tab» Two Tepresent positive design parameters

5 of anti-drift terms of the USV-UAV adaptive law on the yaw, roll and pitch degrees of freedom LV501833 respectively. According to the adaptive cooperative path following control method for USV-UAV based on 3D mapping guidance, the 3D mapping guidance strategy can equivalently map the surface reference path information to the aerial reference surface to establish effective connection between the USV and the UAV. Compared to the prior art, the USV-UAV system can be controlled at the same time, and the problems of structural uncertainty and explosion of the complexity in the USV-UAV cooperative system can be solved by using a fuzzy logic system and dynamic surface control technique. In this way, the automaticity of the USV-UAV in the aspect of cooperative path following can be improved.

BRIEF DESCRIPTION OF THE DRAWINGS In order to illustrate the embodiments of the invention or technical schemes of the prior art more clearly, drawings that are needed for the embodiments or the prior art shall be briefly introduced. Obviously, drawings in the following description are some embodiments of the invention, for those of ordinary skill in the art, other drawings can also be obtained from these drawings without creative efforts. FIG. 1 is a signal flow diagram of the USV-UAV cooperative path following control system of the invention; FIG. 2 is a variable explanatory drawing of the USV-UAV cooperative system of the invention; FIG. 3 is a structural frame drawing of the invention, FIG. 4 is a marine disturbance drawing of the invention under four-class sea conditions; FIG. 5 is a trajectory diagram of the USV-UAV cooperative path following of the invention; FIG. 6 is a control input diagram of the USV-UAV cooperative system of the invention; FIG. 7 1s a position and attitude error diagram of the USV-UAV cooperative system of the invention.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS In order to enable the purposes, technical schemes and advantages of the embodiments of the invention to be clearer, with reference to the drawings of the embodiments of the invention, the technical schemes of the embodiments of the invention shall be described clearly and 6 completely. Obviously, the described embodiments are parts of the embodiments of the LU501833 invention rather than all of the embodiments. Based on the embodiments of the invention, other embodiments that are obtained by those of ordinary skill in the art without creative efforts shall all be within the protection scope of the invention.

FIG. 1 is the signal flow diagram of the USV-UAV cooperative path following control system of the invention, and FIG. 2 is the variable explanatory drawing of the USV-UAV cooperative system of the invention; as shown in FIG. 1 and FIG. 2, the method of the embodiment can comprise the following steps that: S1, models of a USV-UAV cooperative system are established as a controlled object of a to-be-designed controller in the subsequent step; S2, effective connection between a USV and a UAV is established, and position information of a desired ship route designed by a logic virtual ship for the USV is equivalently mapped to an aerial reference surface of the UAV through 3D mapping guidance to obtain a reference heading of the USV on the surface and a reference heading of the UAV on the aerial surface: S3, a USV-UAV position controller and adaptive law are designed, USV-UAV position errors are reduced, the USV-UAV are guided to follow a reference position, and the position controller is subjected to decoupling by using a nonlinear decoupling technique to obtain a reference roll angle and a reference pitch angle of the UAV; S4, a USV-UAV attitude controller and adaptive law are designed, and USV-UAV attitude errors are reduced; SS, the USV-UAV is controlled to complete cooperative path following control operations.

Specifically, mathematic models are basic parts of the control system, in S1, the USV- UAV mathematic models are described and established as the controlled object of the to-be- designed controller in the subsequent step. When the USV-UAV carries out cooperative path following operations, how to enable the USV and the UAV to follow the desired ship route at the same time is a key problem to be solved. In S2, the position information of a desired ship route designed by a Logic Virtual Ship (LVS) for the USV is equivalently mapped to the aerial reference surface of the UAV through 3D mapping guidance to obtain the reference heading of the USV on the surface and the reference heading of the UAV on the aerial surface respectively. The USV-UAV mathematic models are obtained in S1, and the desired reference position and reference heading of the USV-UAV are provided in S2. In S3, by designing the position controller, the USV-UAV position errors are reduced, the USV-UAV is guided to follow the 7 reference position, the position controller in S3 is subjected to decoupling by using the LU901833 nonlinear decoupling technique, and the reference roll angle and the reference pitch angle of the UAV can be further obtained. In S4, by designing the attitude controller, the USV-UAV attitude errors are reduced. Through S3 and S4, the USV-UAV can be controlled to complete the cooperative path following control operations.

The adaptive cooperative path following control method for USV-UAV based on 3D mapping guidance mainly has two characteristics that: (1) according to the guidance algorithm, the 3D mapping guidance strategy can plan the reference attitude or velocity for the UAV in the system in real-time, and equivalently map the planned reference path in real-time to the aerial reference surface of the UAV to establish effective connection between the UAV, the USV and the cruise reference path; (2) the adaptive fuzzy control laws are designed for position parts and attitude parts of the mixed-order USV-UAV system to realize the control of enabling the USV-UAV to effectively follow the reference path.

The Fuzzy Logic System (FLS) and Dynamic Surface Control (DSC) are used in the algorithm to carry out online approximation on the model structural uncertainties of the USV- UAV cooperative system.

The mixed-order USV-UAV nonlinear system is shown in formula (1) and formula (2).

Xs = Us cos(Ps) — vs sin(yhs) ; = us sin(s) + vs cos (hs) TR 0 Xa = Uaxı Ya = Uay, Za = Vaz Pa = Pa ba = Ja Va = Ta Us = pu + tu + u by = pr +o lax = — “Luge += ReFy + dx day = — “Zug, + ZRF + dy, @) lla, = = "Lug, — 9 + = R2F; + duz Pa = 2 Ode — rg, — = da +76 + dwg da = Sp — bo — 120 +i + dye fa = 25 dada — 202 roy + dy Wherein, 8

Fulve) = Zug, = eus — Sy Ju, — LE 00006 fous) = Thug — SE os — 22 |p |p, — 2222 fe) = PET An nn Dr (3) Ry = cos(¢q) sin(0g) cos (Wa) + sin(6) sin (Va) Ry = cos($a) sin(0a) cos (Wa) — sin(0g) sin(y,) Rz = cos(Pa) cos(Wa) Wherein, as shown in FIG. 3, [x Vir Za Par Pa V i j = s, a represent the surge, drift and the heave displacement and the roll, pitch and yaw angle of the USV-UAV cooperative system. v, = [ug, vs, 75]7 represent the surge, drift and yaw velocity of the USV, and v, = [tax Ugy Vaz» Pa Ja ral represent the velocity and rotating angular velocity of the UAV along the ox,0y,0Z axis. Mu, My, M, represent the additional mass of the models, and d;1, dip, diz, I = U, V,T represent the nonlinear damping terms of the models. J, represents the inertia of rotors of the UAV, ky, koy, ko, represent the pneumatic friction coefficients, m represents the mass of the UAV, g represents the gravitational acceleration, Lex, lyy, 177 represent the moment of inertia, kqy, Kay, kg, represent the rotational resistance coefficients, and A, = 0, — 2, + 3 — Q4> y l = 1,2,3,4 are the angular velocity of the rotors. dw = u,v, T, X, Y, Z, ¢, 0, represents the external disturbance force or moment that the USV-UAV cooperative system is subjected to; Fy represents the resultant force of the force Fj, i = 1,2,3,4 of the rotors, T4, Tg, Ty represent the roll, pitch and yaw moment of the UAV, and Ty, T, represent the surge thrust and the turning moment of the USV.

Suppose that the reference path of the USV is planned by the Logic Virtual Ship (LVS) in real-time, as shown in formula (4). Xsy = Us] COS Pg, V1 = Ug SINYg Ps = Ts (4) Wherein, (x4, Vg, Ws) represent the position coordinates and the heading angle of the LVS, and ug, T5; represent the surge velocity and the yaw angular velocity of the LVS.

As shown in FIG. 4, in order to establish the effective connection between the USV and the UAV, the equivalent mapping technique is used to map the position information of the LVS to the aerial reference surface of the UAV to provide real-time reference position information for the UAV, that is, x4 = Xst Val = Ysı, Wherein, the vertical position za; of the UAV is manually set generally; LVS refers to the Logic Virtual Ship, which can provide the reference path of the USV, that is, the LVS can provide real-time target positions for the USV. LVA refers to the Logic 9

Virtual Aircraft, which can provide the reference path of the UAV, and the position information LV501833 of the LVA is obtained according to the LVS.

According to the relationship between the current position and reference path of the USV- UAYV cooperative system, the reference heading signals of the USV-UAV can be obtained as formula (5): Xje = Xj1 7 Xj» Vje = Yj1 7 Yj Via = 0.5[1 — san(x;e)] san(yje) m + arctan(y;e/Xje)»J =s,a (5) The USV-UAV position controller and adaptive law are designed: according to the USV- UAV nonlinear models and the 3D guidance strategy, the derivative of the USV-UAV position errors can be expressed as formula (6), i: = IT — ws cos(ipse) Xae = Xa 7 Xa (6) Yae = Ya 7 Yai Zae = Za — Zal Wherein, se = Ps — Psa, and [] = Xs1 COS (Psa) + Ysı Sinisa) — Vs sine).

In order to reduce the position errors {5er Xae» Yae» Zae, à corresponding virtual controller is designed as formula (7).

ay, = COS(Pse)* (Kse se — 64) + ID ay = —KaxXae + À 5 © = kde + Ja o) Ay, = —KazZae + Zal Wherein, Kg, Fax Kay» Kaz are positive design parameters.

The virtual controller may lead heavy calculation load in the following derivation, therefore, the dynamic surface control is introduced to carry out order reduction on the derivative of the virtual controller, that is, EnBun + Bun = uno Bun(0) = dun (0),n = s, ax, ay,az (8) Wherein, €, is a time constant that is greater than zero, €, represents dynamic surface signals, and dynamic surface errors are as q,, = Bun — un.

Errors are defined as Une = Un — f,,,n = s, ax, ay, az, the derivative of une can be obtained, that is, Use = mg (fu(Vs) + Tu + dwu — Mubus) 9 © = fea) + MIR2E; + dx — Bug, ©) Ugye = f, Va) + m*R,F, + dwy — Pug, Uaze = f2Va) = 9 + M"R2F; + duvz — Pu, 10

Wherein, fu (vs), fx(Va), fy (Va), fz(Va) represent the structure nonlinear terms of the LU501833 models, and can be subjected to online approximation by using the fuzzy logic system.

In order to simplify the control design, three intermediate variables are defined, that 1s, cx = M*R,F; (5 = m"R,F, (10) cz =—g +m RFs Therefore, the USV-UAYV position controller and adaptive law can be designed as formula (11) and formula (12). Ty = —kgy Use + Bus cu Dy, ps (Vs) + (Pre cu 64) cos (Wse) Fr =m [c2 + c3 + (cz + 9)” Cx = —KaxuYaxe + Buax cu D, PalVa) T Xae Cy = —kayullaye + Buay cu D,Pa(Va) T Yae Cz = —KazuYaze + Braz cu D,PalVa) T Zae (11) Dy = You [Use Ps (Vs) — Ou (Du - @,(0))] Dy = Yox [taxePa(Va) - Oo (By — (0) )] D, = Yoy [aye @ava) cu Toy (a, cu ©,(0))] @, = Yoz [Haze Pa (Va) - 007 (Oz - @,(0))] (12) Wherein, Ksw Kaxu Kayw Kazw You Yox Ywy Yoz» Tour Jwx» Twy» Owz are design parameters greater than zero.

The nonlinear decoupling technique 1s used to solve formula (10) to obtain the reference roll angle and pitch angle of the UAV, that is, sin(Vad) Cx — cos(Vaa) Cy Bad = arctan (cost ad) +9 _ cos(Ppad)ex+sin(PYad)cy Oaa = arctan (FE) (13) The USV-UAV attitude controller and adaptive law are designed: in order to control the current USV-UAV attitude to converge to the reference attitude, the attitude errors are defined as Yje, Pae Bae, J = S, a, and derivation is carried out to obtain that, Vie =" Via Pac = Pa — Pad» One = da — Oad (14) In order to reduce the attitude errors Yj, Pac» Ope, j = S, a, the corresponding virtual controller is designed as formula (15). Ars = —ksypse + sar Ara = —KapVae + Pad 11 dpa = —Kaÿ Pac + Pad» Aga = kaolae + baa (15) 57501898 Wherein, kgy, Kay, kag, Ka are control parameters greater than zero.

Similar to the process of position control, in order to avoid the condition that the attitude virtual controller leads to the explosion of the complexity in the subsequent derivation, the dynamic surface control is introduced, that is, EmBm + Bm = Am Pm (0) = a, (0), m = rs, ra, pa, qa (16) Wherein, €,,1s a time constant that is greater than zero, fn represents dynamic surface signals, and dynamic surface errors are as Gn = Pm — Am.

The errors are defined as 5e = 75 — Bre, Tue = Ta — Bras Pac = Pa — Bpar dae = Ja Bao and derivation is carried out, foe = Mr (fes (V5) + Tr + dwr — MrBrs) j = fraWVa) + by Ty + dwy — Pra a7 Pae = fpa(Va) + bpTp + dwg — Ppa ae = faa(Va) + boTe + dwo — Paa Wherein, f-5 (Vs), fra (Va) foa (Va), fga (Va) represent the structure nonlinear terms of the models.

By using the fuzzy logic system, the adaptive technology and the Backstepping technology, the USV-UAV attitude controller and adaptive law are designed as formula (18) and formula (19). Ty = —KorTse + Brs — Br sve) — Pee Ty = byt [-KartTae — DyPalVa) + Pra — Vae] Tp = by [+KapPae — Da PalVa) + Ppa — Pac] Tg = ba*[-Kagae — DePa(Va) + Paa — Pac] (18) D, = Voor [Te 95s (Vs) — Gor (@r — @(0))] Dp = Yoy [Fae Pala) — Joy (ay — ©,(0))] Dp = Yod [Pac Pa (Va) — On (9 — 040) D0 = Yoo[JacPalYa) — Two(@ — Be (0))] (19) Wherein, kg, kor) Kap, Kaq> Yor You Yo Yw8» Our Ow og Owe are design parameters greater than zero.

In order to carry out the USV-UAV cooperative path following operations, four way points CW, (Om, Om), W, (500m, Om), W5(500m, 500m), W,(1000m, 500m) ) are 12 selected to constitute a way-point path. The initial state of the controlled object is as follows: LU501833 [xs (0), vs (0), ws (0), us (0), v5 (0), 15 (0),xa (0), Ya (0), Za (0), Pa (0), Pa (0), 9a (0), Vax (0), ay (0), az (0), Pa (0), Ja (0), ra (0)] =[-10m, 10m, Odeg, Om/s, Om/s, Orad/s, — 10m, 10m, Om,0deg, Odeg, Odeg, Om/s, Om/s, Om/s, Orad/s, Orad/ s, Orad/s] FIG. 5 represents a simulated environment used on the MATLAB simulation platform, that is, the sea surface under four-class sea conditions and the three-dimensional view of the wind velocity at 10m altitude, the curve of the wind direction and the winds and waves on the sea surface. FIG. 6 to FIG. 7 show the simulated results of the USV-UAV cooperative path following in the simulated sea environment respectively. FIG. 6 represents the trajectory curve of USV-UAV cooperative path following, and it is shown in FIG. 6 that the reference path of the USV is planned and obtained according to the way-point information and further equivalently mapped to the aerial reference surface of the UAV to provide voyage reference signals for the UAV.

In addition, compared to a single automatic system of the USV or the UAV, the adaptive cooperative path following control method for USV-UAV based on 3D mapping guidance can enable the USV-UAV to travel at the desired velocity and follow the reference signals at the same time.

FIG. 7 shows the position errors and attitude errors of the USV-UAV cooperative system, and it can be found from FIG. 7 that although the UAV overshoots at the third way point, it effectively follows the reference path in the end.

Overall beneficial effects: 1) the 3D mapping guidance strategy of the adaptive cooperative path following control method for USV-UAV based on the 3D mapping guidance can equivalently map the surface reference path information to the aerial reference surface, and the effective connection between the USV and the UAV can be established. Compared to the prior art, the USV-UAV system can be controlled at the same time, and the problems of structural uncertainties and explosion of the complexity in the USV-UAV cooperative system can be solved by using the fuzzy logic system and dynamic surface control technique. In this way, the automaticity of the USV-UAV in the aspect of cooperative path following can be improved.

2) the USV-UAYV cooperative path following simulation test is carried out in the simulated sea environment, and the effectiveness of the adaptive cooperative path following control method for USV-UAV based on the 3D mapping guidance is verified. As a significant application in the field of air-sea integration, the USV-UAYV cooperative path following control has significant application prospects in the aspect of air-sea cooperative maritime search and 13 rescue.

LU501833 In the end, it should be stated that all the above embodiments are used to illustrate the technical schemes of the invention rather than limit the invention; although the invention has been illustrated in detail with reference to the embodiments, those of ordinary skill in the art should understand that they can still modify the technical schemes described in the embodiments or substitute parts of or all the technical characteristics of the embodiments equivalently; and these modifications and substitutions will not make the essence of the technical schemes derivate from the scopes of the technical schemes of the embodiments of the invention. 14

Claims (5)

CLAIMS LU501833

1. An adaptive cooperative path following control method for USV-UAV based on 3D mapping guidance 1s characterized by comprising the following steps that: S1, models of a USV-UAV cooperative system are established as a controlled object of a to-be-designed controller in the subsequent step; S2, effective connection between a USV and a UAV is established, and position information is equivalently mapped to an aerial reference surface of the UAV through 3D mapping guidance to obtain a reference heading of the USV on the surface and a reference heading of the UAV on the aerial surface; the position information refers to the position information of a desired ship route designed by an logic virtual ship for the USV; S3, a USV-UAV position controller and adaptive law are designed, USV-UAV position errors are reduced, the USV-UAV is guided to follow a reference position, and the position controller is subjected to decoupling by using a nonlinear decoupling technique to obtain a reference roll angle and a reference pitch angle of the UAV; S4, a USV-UAV attitude controller and adaptive law are designed, and USV-UAV attitude errors are reduced; SS, the USV-UAV is controlled to complete cooperative path following control operations.

2. An adaptive cooperative path following control method for USV-UAV based on 3D mapping guidance according to claim 1 is characterized in that the formulas for the models of the USV-UAV cooperative system in S1 are as follows: Xs = U, COS( bg) — V, SIN( by) ys = U, Sin( bg) + V, cos( bg) Xa = Uaxy Ya = Uay, Za = Uaz b, = Pa 9 a = da Va =Ta 15 gp = 209 À 7, dwn LU501833 Mu Mu Mu Vg — fy(vs) + dwv My my I's — frCvs) 4 1 ty + dor mr mr mr . _ Kax 1 Uax = m Yax + m RxFr + dwx . k 1 lay = day + —RyFr + dyy (2) . k, 1 Uaz = — Eu, —g+ —R.F¢ + dwz , — Lyle @ i _ 2er 5 __ Kox 1 2 da Pa === Vata mgm Va ma Pa tin To dw . | ; à Orr 3 Koy à 2 … d da = 2e 4 ba - FH, — 2 dat Te + dw yy yy yy yy ; Iex-lyy à à Koz 42 , d fp =———2¢, 0, —=20D, +— Ty +dwv | Iz 177 — My _ dus, due _ dus 13 ful v s) TT mu Vsfs mu Us mu [us [Us mu Ug m d d d Vs) = my UsT's - my VS - [vs [Vs — mu VS my—m d d d fr( Vs) = = UsVs — erg ma rng — Ze re (3) Ry = cos(¢,) sin( 6 ,) cos( b 4) + sin( 9 4) sin( b 4) R, = cos(¢,) sin( 9 ,) cos( b 4) — sin( 9 ,) sin( +) R, = cos(¢,) cos( b ,) wherein, formula (1) and formula (2) are the models ofthe USV-UAV cooperative system, formula (1) is the kinematic model, formula (2) is the dynamical model, and formula (3) is the . . . T ,

expansion formula for some variables in formula (2); [x Vir Zar Par Jar V i ,j = s,a represent the surge, drift and heave displacement and the roll, pitch and heading angle of the USV-UAV cooperative system; v, = [Us, Vs, T5] represent the surge, drift and yaw velocity of the USV,

T . . . and va = [Uax» Uay» Vaz» Pa Ja ra] represent the velocity and rotating angular velocity of the UAV along the front and rear direction, left and right direction and up and down direction; My, My, M, represent the additional mass of the models, and d;;, d;,, d;3, | = u, V,r represent nonlinear damping terms of the models; J, represents the inertia of rotors of the UAV, Kox» Koy, Koz Tepresent the pneumatic friction coefficients, m represents the mass of the UAV, g represents the gravitational acceleration, I.y,1,y,1,, represent the moment of inertia, k ax, Kay» Kaz represent the rotational resistance coefficients, and A, = 2; — 2, + 23 — (,, £2;,i =1,23,4 are the angular velocity of the rotors. dyi, | = U, V,T, X, Y, Z, D, 0, represents the external disturbance force or moment that the USV-UAV cooperative system is subjected to; Fy represents the resultant force of the force Fj, i = 1,2,3,4 of the rotors, T4, Tg, Ty represent 16 the roll, pitch and yaw moment of the UAV, and 7,,, T, represent the surge thrust and the turning LU501833 moment of the USV.

3. An adaptive cooperative path following control method for USV-UAV based on 3D mapping guidance according to claim 1 is characterized in that for the effective connection between the USV and the UAV established in S2: the reference path of the USV is as follows: sy = Us] COS Pg, Ÿs1 = Us] SIN Us Pg = Ty (4) wherein, (xg, Vg, Ws) represent the position coordinates and the heading angle of the logic virtual ship, and ug, 75; represent the surge velocity and the yaw angular velocity of the logic virtual ship. In order to establish the effective connection between the USV and the UAV, an equivalent mapping technique is used to map the position information of the logic virtual ship to the aerial reference surface of the UAV to provide real-time reference position information for the UAV, that is, Xa1 = Xst Yu = Vs, Wherein, the vertical position za; of the UAYV is manually set generally; according to the relationship between the current position and reference path of the USV- UAV cooperative system, reference heading signals of the USV-UAV are calculated: Xje = Xj1 7 Xj» Yje = Vj1 — Yj Vja = 0.5[1 — san(x;e)] sgn(vje) x + arctan(y;e/Xxje), j = s,a (5) wherein, Xje, Yje, j = S, a represent the distances between current position coordinates to desired position coordinates of the USV and the UAV respectively; Wa, } = S, a represent reference heading angles of the USV and the UAV respectively; sgn ( * ) represents a sign function.

4. An adaptive cooperative path following control method for USV-UAV based on 3D mapping guidance according to claim 1 is characterized in that the USV-UAV position controller designed in S3 is as follows: Ty = Ksullse + Bus — Dus (ve) + (2 — 84 ) cosh) Cx = —KaxuYaxe + Buax — OxPalVa) — Xae Cy = —KayuYaye + Buay — @yPa(Va) — Yae cz = ~Kazullaze + Buaz — ®20a(Va) — ZaeTu (6) the USV-UAY adaptive law designed in S3 is as follows: 17

Ou = Vou[Use Ps (Vs) — Tou (By — Bu(0))] HUS01833 Bx = Vor [UaxePa(Va) — Tur (@x — Bx(0))] By = Voy [UayePa(Va) = Foy (@y — B,(0))] Oz = Yoz|Yaze PalVa) — 9wz(@ — @,(0))] (7) wherein, ug represents the difference value between the velocity and the virtual velocity control law of the USV, Bys represents dynamic surface signals of the virtual velocity control law of the USV, £, represents the linear distance of the USV to the reference position, and yg, represents the heading errors of the USV. ¢, C,, Cz represent intermediate control variables that are used to simplify control, Ugye, Ugye, Vaze Tepresent the difference values between the velocity on the x-axis, y-axis and z-axis directions and the virtual velocity control laws of the UAV respectively, Buax» Puay» Puaz Tepresent the dynamic surface signals of the virtual velocity control law respectively, and Xae» Vae Zae represent the distance differences from the current coordinates on the x-axis, y-axis and z-axis of the UAV to the desired position coordinates respectively. kg, Kaxur Fayu Kazu Tepresent positive controller parameters of the USV-UAV on the surge degree of freedom, x-axis, y-axis and z-axis directions respectively, You Yoox Yoy Yoz TEPresent positive design parameters of the adaptive law of the USV-UAV on the surge degree of freedom, x-axis, y-axis and z-axis directions respectively, Oww Tax) Owyr Owz Tepresent positive design parameters of the anti-drift terms of adaptive law of the USV-UAV on the surge degree of freedom, x-axis, y-axis and z-axis directions respectively, ¢,(v,) and p,(V;) represent fuzzy basic functions, w;, i = u, x, y, Z represents adaptive parameters of a fuzzy system, and &;,i = u, x, y, Z represents observation values of adaptive parameters of the fuzzy system.

5. An adaptive cooperative path following control method for USV-UAV based on 3D mapping guidance according to claim 1 is characterized in that the USV-UAV attitude controller designed in S4 is as follows: ty = —KorTse + Bre — B05 (vs) — Wie Ty = by‘ [-Kartae — DyPa (Va) + Bra — Vae] Tp = by [+KapPae — Da PalVa) + Ppa — Pac] To = ba‘|-KagYae — ®oPa(Va) + Baa — Pac] (8) the USV-UAV adaptive law designed in S4 is as follows: D, = Voor [Tres (Vs) — For (@r — @(0))] 18

Dp = Yoy [Fae Pala) — Oy (ay — ©,(0))] 7901808 Dp = Yod [Pac Pa (Va) — On (9 — 040) de = Yoo[JacPa(Ya) — Kwe(@e — Dg (0))] (9) wherein, r,e represents the difference value between the yaw velocity and the virtual yaw velocity control law of the USV, Pr represents dynamic surface signals of the virtual yaw velocity control law of the USV, and WP, represents heading errors of the USV.

Tae,DPae ae represent difference values between the yaw, roll and pitch velocity on the x-axis, y-axis and z-axis direction and the corresponding virtual velocity control laws of the UAV respectively, Bras Ppa Pqa represent dynamic surface signals of virtual yaw, roll and pitch velocity control law respectively, and Wae, Paer 040 represent difference values between the current heading angle, roll angle and pitch angle and the desired heading angle, roll angle and pitch angle of the UAV respectively; Ksr, Kar, Kap, Kaq represent positive controller parameters of the USV- UAV on the yaw, roll and pitch degrees of freedom respectively, Yur, Yay Yod» Yoo Tepresent positive design parameters of the USV-UAV adaptive control law on the yaw, roll and pitch degrees of freedom respectively, and Jar» Guy) Top» Owe Tepresent positive design parameters of anti-drift terms of the USV-UAV adaptive law on the yaw, roll and pitch degrees of freedom respectively. 19