CN116339355B

CN116339355B - Underwater vehicle and formation tracking control method and device thereof

Info

Publication number: CN116339355B
Application number: CN202310205875.4A
Authority: CN
Inventors: 高宏宇; 张木; 薛任宇欣; 张育玮; 孔凡忠; 王锐; 赵宏亮; 黄磊; 刘鹏
Original assignee: Xinxing Jihua Beijing Intelligent Equipment Technology Research Institute Co ltd
Current assignee: Xinxing Jihua Beijing Intelligent Equipment Technology Research Institute Co ltd
Priority date: 2023-03-03
Filing date: 2023-03-03
Publication date: 2023-10-20
Anticipated expiration: 2043-03-03
Also published as: CN116339355A

Abstract

The invention provides an underwater vehicle and a formation tracking control method and device thereof, wherein the formation tracking error of the underwater vehicle is determined according to a navigation track and an expected time-varying formation; determining a kinematic uncertainty term of the underwater vehicle based on the influence of the ocean current interference on the underwater vehicle formation tracking control; and performing formation tracking control on the underwater vehicle based on the kinematics uncertainty item and the formation tracking error. The invention compensates the uncertain items of the kinematics of the underwater vehicle in the formation tracking control of the underwater vehicle, thereby improving the formation tracking capacity and the disturbance rejection capacity of the underwater vehicle.

Description

Underwater vehicle and formation tracking control method and device thereof

Technical Field

The invention relates to the technical field of underwater vehicle control, in particular to an underwater vehicle and a formation tracking control method and device thereof.

Background

Compared with the exploration of land, space and even outer space by human beings, the understanding, development and utilization of the underwater field by human beings are far from sufficient. With the rapid development of artificial intelligence technology, underwater vehicles have been applied to underwater exploration and search, military reconnaissance and combat battle, underwater pipeline maintenance, sunken ship archaeology, and the like. However, in the face of a wide and complex underwater environment, a single underwater vehicle is limited by factors such as energy storage, underwater communication, positioning and the like, the operation range and effect are limited, the information acquisition type is few, and the exploration needs to be completed through an underwater vehicle array.

However, the existing underwater vehicle formation control has the following defects: (1) Most underwater vehicle arrays adopt fixed teams, and the team transformation capability facing complex environments is poor; (2) The influence of ocean current interference on the underwater vehicle formation tracking is not fully considered, and a motion model of the underwater vehicle is difficult to accurately establish, so that the existing underwater vehicle formation control technology is difficult to adapt to the increasingly complex and diversified exploration task requirements. Therefore, the development of the three-dimensional time-varying underwater vehicle formation control technology has great engineering practice significance.

Disclosure of Invention

The invention provides an underwater vehicle and a formation tracking control method and device thereof, which are used for solving the defects of poor three-dimensional time-varying formation transformation capability and insufficient anti-interference capability of the underwater vehicle in the complex environment in the prior art and improving the three-dimensional time-varying formation tracking control capability of the underwater vehicle.

In a first aspect, the present invention provides a method for tracking and controlling formation of an underwater vehicle, suitable for a follower underwater vehicle, the method comprising:

acquiring a navigation track and an expected time-varying formation;

determining an expected control interval between the expected time-varying formation and the navigation track according to the expected time-varying formation;

Determining a formation tracking error according to the expected control interval, the dynamic parameters of the navigation track and the dynamic parameters of the follower underwater vehicle;

determining a kinematic uncertainty term from nonlinear disturbances generated by non-zero angle of attack and sideslip angle and the desired control pitch;

and performing formation tracking control based on the kinematics uncertainty item and the formation tracking error.

In a second aspect, the present invention provides a method for tracking and controlling formation of underwater vehicles, suitable for piloters of underwater vehicles, the method comprising:

acquiring a navigation track and an expected time-varying formation;

determining the navigation track of each follower underwater vehicle according to the navigation track and the expected time-varying formation;

determining a formation tracking error of each follower underwater vehicle according to the navigation track of each follower underwater vehicle and the received dynamic parameters of each follower underwater vehicle;

determining a kinematic uncertainty term of each of the follower underwater vehicles according to the received nonlinear interference generated by the non-zero attack angle and sideslip angle of each of the follower underwater vehicles;

and performing formation tracking control on each follower underwater vehicle based on the kinematics uncertainty item and the formation tracking error.

In a third aspect, the present invention provides an underwater vehicle formation tracking control device adapted for use with a follower underwater vehicle, the device comprising:

the first acquisition module is used for acquiring a navigation track and an expected time-varying formation;

the expected control interval determining module is used for determining an expected control interval between the expected control interval and the navigation track according to the expected time-varying formation;

the first formation tracking error determining module is used for determining a formation tracking error according to the expected control interval, the dynamic parameters of the navigation track and the dynamic parameters of the follower underwater vehicle;

a first kinematic uncertainty term determination module for determining a kinematic uncertainty term based on non-linear disturbances generated by non-zero angle of attack and sideslip angle and the desired control pitch;

and the first formation tracking control module is used for performing formation tracking control based on the kinematics uncertainty item and the formation tracking error.

In a fourth aspect, the present invention provides an underwater vehicle formation tracking control device adapted for use with a pilot underwater vehicle, the device comprising:

the second acquisition module is used for acquiring a navigation track and an expected time-varying formation;

The navigation track determining module is used for determining the navigation track of each follower underwater vehicle according to the navigation track and the expected time-varying formation;

the second formation tracking error determining module is used for determining the formation tracking error of each follower underwater vehicle according to the navigation track of each follower underwater vehicle and the received dynamic parameters of each follower underwater vehicle;

a second kinematic uncertainty term determination module for determining a kinematic uncertainty term for each of the follower underwater vehicles based on the received non-linear disturbances generated by the non-zero angle of attack and sideslip angle of each of the follower underwater vehicles;

and the second formation tracking control module is used for performing formation tracking control on each follower underwater vehicle based on the kinematics uncertainty item and the formation tracking error.

In a fifth aspect, the present invention provides an underwater vehicle comprising an underwater vehicle formation tracking control device according to the third or fourth aspect.

The invention provides a formation tracking control method and a formation tracking control device for an underwater vehicle, which are suitable for a follower underwater vehicle and comprise the following steps: acquiring a navigation track and an expected time-varying formation; the navigation trajectory is a navigation trajectory of a pilot underwater vehicle associated with the follower underwater vehicle; determining an expected control interval between the navigation track and the target control interval according to the expected time-varying formation; determining a formation tracking error according to the expected control interval, the dynamic parameters of the navigation track and the dynamic parameters of the underwater vehicle of the follower; determining a kinematic uncertainty term from nonlinear disturbances generated by non-zero angle of attack and sideslip angle and the desired control pitch; and performing formation tracking control based on the kinematics uncertainty item and the formation tracking error. When the follower underwater vehicle carries out formation tracking control on the follower underwater vehicle, the invention compensates the uncertain item of the kinematics of the follower underwater vehicle, improves the disturbance rejection capability of the follower underwater vehicle, and further enhances the formation tracking capability of the follower underwater vehicle. In addition, the invention introduces the expected time-varying formation, thereby being capable of adjusting the formation type in real time according to the requirement and meeting the task requirement of complex and severe environment.

The invention provides a formation tracking control method and a formation tracking control device for an underwater vehicle, which are suitable for a pilot underwater vehicle and comprise the following steps: acquiring a navigation track and an expected time-varying formation; determining the navigation track of each follower underwater vehicle according to the navigation track and the expected time-varying formation; determining a formation tracking error of each follower underwater vehicle according to the navigation track of each follower underwater vehicle and the received dynamic parameters of each follower underwater vehicle; determining a kinematic uncertainty term of each of the follower underwater vehicles according to the received nonlinear interference generated by the non-zero attack angle and sideslip angle of each of the follower underwater vehicles; and performing formation tracking control on each follower underwater vehicle based on the kinematics uncertainty item and the formation tracking error. When the pilot underwater vehicle carries out formation tracking control on the follower underwater vehicle, the invention compensates the uncertain item of the kinematics of the follower underwater vehicle, improves the disturbance rejection capability of the follower underwater vehicle, and further enhances the formation tracking capability of the follower underwater vehicle. In addition, the invention introduces the expected time-varying formation, thereby being capable of adjusting the formation type in real time according to the requirement and meeting the task requirement of complex and severe environment.

Drawings

In order to more clearly illustrate the invention or the technical solutions of the prior art, the following description will briefly explain the drawings used in the embodiments or the description of the prior art, and it is obvious that the drawings in the following description are some embodiments of the invention, and other drawings can be obtained according to the drawings without inventive effort for a person skilled in the art.

FIG. 1 is a flow chart of an underwater vehicle formation tracking control method suitable for a follower underwater vehicle provided by the invention;

FIG. 2 is a schematic flow chart of an underwater vehicle formation tracking control method suitable for a pilot underwater vehicle;

FIG. 3 is a schematic representation of a coordinate system constructed in accordance with the present invention;

FIG. 4 is a block diagram of three-dimensional space multi-underwater vehicle formation control provided by the invention;

FIG. 5 is a simulation diagram of three-dimensional motion trajectories of an underwater vehicle formation provided by the invention;

FIG. 6 (a) is a schematic diagram of the horizontal axis error of an underwater vehicle formation track provided by the present invention;

FIG. 6 (b) is a schematic view of the error on the vertical axis of the underwater vehicle formation track provided by the present invention;

FIG. 6 (c) is a schematic view of vertical axis error of an underwater vehicle formation track provided by the present invention;

FIG. 7 is a schematic diagram of an underwater vehicle formation tracking control device adapted for use with a follower underwater vehicle in accordance with the present invention;

fig. 8 is a schematic structural diagram of an underwater vehicle formation tracking control device suitable for a pilot underwater vehicle.

Detailed Description

For the purpose of making the objects, technical solutions and advantages of the present invention more apparent, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings, and it is apparent that the described embodiments are some embodiments of the present invention, not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.

The underwater vehicle and its formation tracking control method and apparatus of the present invention are described below with reference to fig. 1-8.

In a first aspect, the present invention provides a method for tracking and controlling formation of an underwater vehicle, suitable for a follower underwater vehicle, as shown in fig. 1, the method comprising:

s11: acquiring a navigation track and an expected time-varying formation;

wherein, the underwater vehicle mainly refers to an autonomous underwater vehicle which has autonomous control capability and can perform independent operation, such as an autonomous underwater vehicle (Autonomous Underwater Vehicle, AUV) and an unmanned underwater vehicle (Unmanned Underwater Vehicle, UUV) with automatic control capability; the semi-autonomous underwater vehicle can be flexibly switched between manual control operation and independent operation.

The navigation track and the expected time-varying formation engineer are designed according to the field working conditions and engineering experience;

the navigation trajectory is a navigation trajectory of a pilot underwater vehicle associated with the follower underwater vehicle;

the navigation track comprises two conditions of an expected navigation track and an actual navigation track, and is applicable to two piloting scenes;

first kind of pilot scene: the navigation system has a unique navigator underwater vehicle, and all the follower underwater vehicles take the navigator underwater vehicle as a navigator. The pilot underwater vehicle may be a virtual body or an entity. When the pilot underwater vehicle is a virtual body, defaulting that the expected navigation track and the actual navigation track corresponding to the pilot underwater vehicle are the same, and the pilot underwater vehicle is a preset navigation track, and all the underwater vehicles in the formation are the follower underwater vehicles. When the pilot underwater vehicle is an entity, it should be any underwater vehicle in the formation, where the other underwater vehicles in the formation are all follower underwater vehicles.

Second pilot scenario: all underwater vehicles in the formation have corresponding grades, the K-th underwater vehicle takes the K-1-th underwater vehicle corresponding to the K-th underwater vehicle as a pilot, and only one 1-th underwater vehicle exists. The class 1 underwater vehicle may be a virtual body or an entity. When the 1 st-level underwater vehicle is a virtual body, defaulting that the corresponding expected navigation track is the same as the actual navigation track is a preset navigation track. When the class 1 underwater vehicle is an entity, it should be any underwater vehicle in the formation, K being a natural number not less than 2.

S12: determining an expected control interval between the expected time-varying formation and the navigation track according to the expected time-varying formation;

it can be understood that the expected control interval between the navigation track and the navigation track is the virtual point corresponding to the follower underwater vehicle on the navigation track and the expected control interval between the follower underwater vehicle.

Generally, virtual points corresponding to the follower underwater vehicle on the navigation track are determined by a preset positioning mode;

for example: setting points on the navigation track and navigation time to have a one-to-one correspondence, and positioning corresponding virtual points on the navigation track according to the current navigation time of the underwater vehicle of the follower;

or establishing a one-to-one correspondence between the follower underwater vehicle and the points on the navigation track in a corresponding labeling mode so as to position the virtual points corresponding to the follower underwater vehicle on the navigation track.

S13: determining a formation tracking error according to the expected control interval, the dynamic parameters of the navigation track and the dynamic parameters of the follower underwater vehicle;

s14: determining a kinematic uncertainty term from nonlinear disturbances generated by non-zero angle of attack and sideslip angle and the desired control pitch;

S15: and performing formation tracking control based on the kinematics uncertainty item and the formation tracking error.

The invention provides a formation tracking control method of an underwater vehicle, which is suitable for a follower underwater vehicle and comprises the following steps: acquiring a navigation track and an expected time-varying formation; determining an expected control interval between the navigation track and the target control interval according to the expected time-varying formation; determining a formation tracking error according to the expected control interval, the dynamic parameters of the navigation track and the dynamic parameters of the underwater vehicle of the follower; determining a kinematic uncertainty term from nonlinear disturbances generated by non-zero angle of attack and sideslip angle and the desired control pitch; and performing formation tracking control based on the kinematics uncertainty item and the formation tracking error. When the follower underwater vehicle carries out formation tracking control on the follower underwater vehicle, the invention compensates the uncertain item of the kinematics of the follower underwater vehicle, improves the disturbance rejection capability of the follower underwater vehicle, and further enhances the formation tracking capability of the follower underwater vehicle. In addition, the invention introduces the expected time-varying formation, thereby being capable of adjusting the formation type in real time according to the requirement and meeting the task requirement of complex and severe environment.

In addition, the method can be used for formation control of the underwater vehicle in the two-dimensional plane, and can also be applied to a three-dimensional space with stronger nonlinearity and uncertainty by constructing a three-dimensional expected time-varying formation.

Specifically, before S11, the method further includes: a pre-built kinetic controller is obtained. The dynamics controller is constructed according to the following process:

(one): constructing a coordinate system:

the invention constructs a geodetic coordinate system { I }, a carrier coordinate system { B } of the follower underwater vehicle, a resultant velocity coordinate system { W } of the follower underwater vehicle and a navigation coordinate system.

Geodetic coordinate system { I }, with the notation O-x _I y _I z _I The coordinate origin O of the representation is positioned at a point of a sea level which is relatively fixed in the earth, and x _I The axis points to north, y _I The axis is directed to the east, z _I The axis points to the earth center;

the carrier coordinate system { B } of the follower underwater vehicle is denoted by the symbol M _B -x _B y _B z _B Representing the origin of coordinates thereof and the centre of gravity M of the follower underwater vehicle _B Overlap, x _B The axis is directed to the bow of the follower underwater vehicle, y _B The axis pointing to the starboard, z of the follower underwater vehicle _B The axis is determined by the right hand spiral rule;

the co-velocity coordinate system { W } of the follower underwater vehicle is denoted by the symbol M _B -x _W y _W z _W Representation of the origin of coordinates thereof with the centre of gravity of the follower underwater vehicleOverlap, x _W Axis along tangential direction of the follower underwater vehicle track (i.e. the resultant speed direction of the follower underwater vehicle), y _W Horizontal plane x with axis parallel to the geodetic coordinate system _I Oy _I ，z _W The axis is determined by the right hand spiral rule;

navigation coordinate system: the origin of coordinates coincides with a virtual point Q corresponding to the follower underwater vehicle on the navigation track, the transverse axis is along the tangential direction of the track at the virtual point Q, and the longitudinal axis is parallel to the horizontal plane x of the geodetic coordinate system _I Oy _I The vertical axis is determined by the right hand spiral law.

(II): and modeling formation tracking control of the formation of the unmanned underwater vehicle in the three-dimensional space under the condition of multiple uncertainties in the coordinate system.

First, based on the assumption that the pitch angle of the unmanned underwater vehicle is limited and the roll angle and the roll angular velocity are about zero, the kinematic model of the follower underwater vehicle can be simplified to:

in the above description, the position coordinate of the follower underwater vehicle in the geodetic coordinate system is p= [ x, y, z] ^T θ is the pitch angle of the follower underwater vehicle, i.e. the carrier coordinate system { B } around y of the follower underwater vehicle _B The rotation angle of the shaft when the shaft rotates to be parallel to the geodetic coordinate system { I }, wherein ψ is the heading angle of the follower underwater vehicle, namely the carrier coordinate system { B } of the follower underwater vehicle is wound around z _B The axis rotates to a rotation angle parallel to the geodetic coordinate system { I }, q is the pitch angle speed of the follower underwater vehicle under the carrier coordinate system { B }; r is the heading angular velocity of the follower underwater vehicle in a carrier coordinate system { B }, u, v and w are x of the follower underwater vehicle along the carrier coordinate system { B }, respectively _B Axis, y _B Axis and z _B The translational speed of the shaft, the points on the parameter represent deriving the parameter.

Then, considering the influence of the non-zero variable attack angle and sideslip angle of the follower underwater vehicle on the formation tracking, and constructing a formation tracking error dynamics model of the follower underwater vehicle;

the formation control aims at keeping a virtual point Q corresponding to the follower underwater vehicle and the follower underwater vehicle on a navigation track at a desired control distance d= [ d ] _x ，d _y ，d _z ] ^T Form tracking error e of the follower underwater vehicle under the navigation coordinate system _p The expression of (2) is as follows:

in the above formula, the position coordinate of the virtual point Q in the geodetic coordinate system is p ₀ ＝[x ₀ ，y ₀ ，z ₀ ] ^T ，Rotation matrix from geodetic coordinate system to navigational coordinate system,>

wherein the matrix is rotatedSatisfy->Then->The method comprises the following steps:

in the above formula, χ ₀ For the track angle of the virtual point Q, i.e. the angle of rotation when the navigational coordinate system is rotated about its vertical axis to be parallel to the geodetic coordinate system { I }, v ₀ Is the angle of latency of the virtual point Q, i.e. the angle of rotation when the navigational coordinate system is rotated about its longitudinal axis to be parallel to the geodetic coordinate system I.

e _p The time derivative is as follows:

In the above, ψ _e For the error rotation angle corresponding to the heading angle of the follower underwater vehicle, namely the carrier coordinate system { B } of the follower underwater vehicle is wound around y _B The axis rotates to a rotation angle theta parallel to the navigation coordinate system _e For the error rotation angle corresponding to the pitch angle of the follower underwater vehicle, namely the carrier coordinate system { B } of the follower underwater vehicle is wound around z _B The shaft rotates to a rotation angle parallel to the navigation coordinate system,is a rotation matrix from the geodetic coordinate system { I } to the navigational coordinate system,/and }>A rotation matrix, v, for a carrier coordinate system { B } to a geodetic coordinate system { I } of the follower underwater vehicle ₀ For the sum velocity vector of the virtual point Q in the navigation coordinate system, v _B Representing the velocity vector, v, of the follower underwater vehicle in its carrier coordinate system { B }, v _w And a resultant velocity vector representing the follower underwater vehicle in its resultant velocity coordinate system { W }.

In the derivation process, nonlinear interference generated by tracking control of three-dimensional time-varying formation by using non-zero attack angle and sideslip angle of the follower underwater vehicle under a navigation coordinate systemDerivative +.A. of the follower underwater vehicle and the desired control distance to the follower underwater vehicle in a navigation coordinate system >Is the integrated result of (2)A kinematic uncertainty term representing the follower underwater vehicle.

Thus, the first and second substrates are bonded together,can be further developed as:

in the above, f _p ＝[f _x ,f _y ,f _z ] ^T ，f _x 、f _y And f _z Respectively f _p The transverse axis component, the longitudinal axis component and the vertical axis component of the carrier coordinate system of the follower underwater vehicle, U is the total speed of the follower underwater vehicle, U ₀ Is the combined velocity of the virtual point Q in the geodetic coordinate system.

The invention obtains the expression of the uncertain item of the kinematics of the follower underwater vehicle by simplifying and deducing the formation tracking error dynamics model of the follower underwater vehicle, and lays a foundation for the compensation of the uncertain item of the follow-up kinematics.

Then, an improved line-of-sight guidance law is designed based on the kinematic uncertainty term of the follower underwater vehicle.

The structures are respectively used for counteracting f _y And f _z Is a first auxiliary variable delta of (1) _y And a second auxiliary variable delta _z ；δ _y And delta _z The following equation is satisfied:

wherein, the liquid crystal display device comprises a liquid crystal display device,

unfolding the delta _y And delta _z Expression of (2), yields:

in the above-mentioned method, the step of,γ _z ＝f _z /U，Δ _y >0，Δ _z >0，/>Δ _y for a first line of sight distance, delta, of the follower underwater vehicle _z A second line-of-sight distance for the follower underwater vehicle.

Constructing a desired line of sight angle based on the first auxiliary variable and the second auxiliary variable; the expected sight angle comprises an expected error rotation angle corresponding to the heading angle of the follower underwater vehicle and an expected error rotation angle corresponding to the pitch angle of the follower underwater vehicle; the expression of the desired line of sight angle is:

Wherein, the liquid crystal display device comprises a liquid crystal display device,for the desired error rotation angle corresponding to the pitch angle of the follower underwater vehicle,/for the desired error rotation angle>And (3) rotating the angle for the expected error corresponding to the heading angle of the follower underwater vehicle.

Constructing an expected pitch angle and an expected heading angle of the follower underwater vehicle based on a coordinate conversion equivalence principle and the expected line-of-sight angle;

namely: to make the error rotation angle (psi) _e ，θ _e ) Respectively towards the angle of rotation of the expected errorAccording to the principle of coordinate transformation equivalence->And combining corresponding rotation matrix calculation to obtain the following steps:

in the above, θ ^d For a desired pitch angle, ψ, of the follower underwater vehicle ^d For a desired heading angle of the follower underwater vehicle χ _d For a desired track angle, v, of the follower underwater vehicle _d Is the desired voyage angle of the follower underwater vehicle.

Here the number of the elements is the number,

the invention skillfully converts the compensation of the uncertain items of the kinematics of the follower underwater vehicle into the control of the attitude angle of the follower underwater vehicle, thereby enabling the controller to realize the compensation target more easily.

And finally, on the basis of a follower underwater vehicle formation tracking error dynamics model and a sight line guidance law, designing a dynamics controller for guiding the pitch angle and the heading angle of the follower underwater vehicle to reach expected values.

The expression of the kinetic controller is as follows:

in the above, u _d For the expected sailing speed of the follower underwater vehicle along the transverse axis of the carrier coordinate system of the follower underwater vehicle, q _d For the desired pitch speed of the follower underwater vehicle in its carrier coordinate system, r _d For a desired heading angular velocity of the follower underwater vehicle in its carrier coordinate system, α is an angle of attack of the follower underwater vehicle, β is a sideslip angle, k of the follower underwater vehicle _d For the first controller gain, k _θ For the second controller gain, k _ψ Gain for the third controller.

It should be noted that the kinematic controller is a programming program that encapsulates the control algorithm.

According to the invention, the kinematics uncertainty item of the follower underwater vehicle is compensated by designing the kinematics controller, so that the anti-interference capability of the follower underwater vehicle is improved. Compared with the existing controller, the dynamic controller is more robust.

Specifically, in S13, the kinetic parameters include, but are not limited to: center of gravity position coordinates, pitch angle, heading angle, and speed of travel along the horizontal, vertical and vertical axes of its carrier coordinate system. In practical use, the kinetic parameters may be acquired by sensors onboard the underwater vehicle.

The step S13 includes:

positioning corresponding virtual points on the navigation track based on navigation time;

recording the deviation between the position coordinates of the follower underwater vehicle at the navigation time and the position coordinates of the virtual point as a first deviation;

recording the deviation between the first deviation and the expected control pitch as a second deviation;

taking the product of the second deviation and a rotation matrix from the geodetic coordinate system to the navigation coordinate system as a formation tracking error.

It will be appreciated that the foregoing formulaNamely the S13 formula expression.

The invention provides a calculation mode of the formation tracking error of the follower underwater vehicle during application, and provides a basis for formation control of the follower underwater vehicle.

Specifically, the kinematic uncertainty term in S14 may be according to the foregoing formula To calculate.

Specifically, the step S15 includes:

s15.1, performing formation tracking control based on the formation tracking error, the kinematic uncertainty item and a dynamics controller;

further, S15.1 includes:

determining an attack angle and a sideslip angle;

determining a track angle and a hidden angle of the virtual point;

calculating the combined speed under the carrier coordinate system according to the navigation speeds along the horizontal axis, the vertical axis and the vertical axis of the carrier coordinate system;

Calculating the combined speed of the virtual points according to the navigation speeds of the virtual points along the transverse axis, the longitudinal axis and the vertical axis of the carrier coordinate system;

substituting the formation tracking error, the kinematics uncertainty item, the attack angle, the sideslip angle, the pitch angle, the heading angle and the combination speed under a carrier coordinate system, and the latency angle, the track angle and the combination speed of the virtual point into the dynamics controller to obtain an output value;

and carrying out formation tracking control according to the output value.

The method comprises the steps that the influence of a non-zero variable attack angle and a sideslip angle generated by the interference of ocean currents on tracking control of a time-varying formation of the underwater vehicle of a follower is taken as a kinematic uncertainty item of the underwater vehicle of the follower together, and a formation tracking error dynamics model is constructed; and then designing a line-of-sight guidance law by taking the compensated kinematics uncertainty item as a target, and finally designing a dynamics controller on the basis of the formation tracking error dynamics model and the line-of-sight guidance law. When the dynamics controller is used for formation tracking control of the follower underwater vehicle, the time-varying formation tracking capability and disturbance rejection capability of the follower underwater vehicle are remarkably improved due to the compensation of the kinematics uncertainty item.

In addition, under the conditions that the three-dimensional formation control algorithm is complicated and the calculation cost is high, the time-varying track tracking control of the formation of multiple underwater vehicles is realized, the defect of inspection of a single underwater vehicle is overcome, and the inspection efficiency and the detection precision are improved.

In a second aspect, the present invention provides a method for tracking and controlling formation of underwater vehicles, suitable for a pilot underwater vehicle, as shown in fig. 2, the method comprising:

s21: acquiring a navigation track and an expected time-varying formation;

an underwater vehicle mainly refers to an autonomous underwater vehicle having autonomous control capability and capable of performing independent operations, such as an AUV (autonomous underwater vehicle) and a UUV (unmanned underwater vehicle) having autonomous control capability; the semi-autonomous underwater vehicle can be flexibly switched between manual control operation and independent operation.

it can be understood that the navigation track is a navigation track of the pilot underwater vehicle, and the navigation track can be a desired navigation track of the pilot underwater vehicle or an actual navigation track of the desired navigation track;

The pilot underwater vehicle may be a physical or virtual controller. When the pilot underwater vehicle is a virtual controller, defaulting that the expected navigation track and the actual navigation track corresponding to the pilot underwater vehicle are the same, and the pilot underwater vehicle is a preset navigation track, and all the underwater vehicles in the formation are the follower underwater vehicles. When the pilot underwater vehicle is an entity, it should be any underwater vehicle in the formation, where the other underwater vehicles in the formation are all follower underwater vehicles.

S22: determining the navigation track of each follower underwater vehicle according to the navigation track and the expected time-varying formation;

it will be appreciated that the navigation track for each follower underwater vehicle may be obtained by coordinate calculation given the navigation track and the desired time-varying formation.

S23: determining a formation tracking error of each follower underwater vehicle according to the navigation track and the dynamic parameters of each follower underwater vehicle;

s24: determining a kinematic uncertainty term of each of the follower underwater vehicles according to the received nonlinear interference generated by the non-zero attack angle and sideslip angle of each of the follower underwater vehicles;

S25: and performing formation tracking control on each follower underwater vehicle based on the kinematics uncertainty item and the formation tracking error.

Assuming that the pilot underwater vehicle is a solid and the navigation trajectory is the desired navigation trajectory of the pilot underwater vehicle, the pilot underwater vehicle may also control itself to navigate according to its desired navigation trajectory through a controller. The controller is also a programming program that encapsulates the control algorithm.

The invention provides a formation tracking control method and a formation tracking control device for an underwater vehicle, which are suitable for a pilot underwater vehicle and comprise the following steps: acquiring a navigation track and an expected time-varying formation; determining the navigation track of each follower underwater vehicle according to the navigation track and the expected time-varying formation; determining a formation tracking error of each follower underwater vehicle according to the navigation track and the dynamic parameters of each follower underwater vehicle; determining a kinematic uncertainty term of each of the follower underwater vehicles according to the received nonlinear interference generated by the non-zero attack angle and sideslip angle of each of the follower underwater vehicles; and performing formation tracking control on each follower underwater vehicle based on the kinematics uncertainty item and the formation tracking error. When the pilot underwater vehicle carries out formation tracking control on the follower underwater vehicle, the invention compensates the uncertain item of the kinematics of the follower underwater vehicle, improves the disturbance rejection capability of the follower underwater vehicle, and further enhances the formation tracking capability of the follower underwater vehicle. In addition, the invention introduces the expected time-varying formation, thereby being capable of adjusting the formation in real time according to the requirement and meeting the task requirement of complex and severe environment.

Specifically, before S21, the method further includes: a pre-built kinetic controller is obtained. The dynamics controller is constructed according to the following process:

(one): constructing a coordinate system:

for ease of description, the subscript i is taken to denote the variable associated with the ith follower underwater vehicle, i e (1-N), N being the total number of follower underwater vehicles in the formation.

The invention constructs a carrier coordinate system { B of an I-th follower underwater vehicle and a geodetic coordinate system { I } _i Co-velocity coordinate system of ith follower underwater vehicle { W } _i And the navigation coordinate system of the ith follower underwater vehicle.

Geodetic coordinate system { I }, with the notation O-x _I y _I z _I Representing that its origin of coordinates O is located at a relatively largely fixed sea levelAt some point, x _I The axis points to north, y _I The axis is directed to the east, z _I The axis points to the earth center;

carrier coordinate system { B of ith follower underwater vehicle _i By symbols }, withIndicating that the origin of coordinates is +.>Coincide with (I)>The axis pointing towards the bow of the i-th follower underwater vehicle,/- >The axis pointing to the starboard side of the ith follower underwater vehicle,>the axis is determined by the right hand spiral rule;

coordinate system { W of combined speed of ith follower underwater vehicle _i By symbols }, withIndicating that the origin of coordinates is +.>Coincide with (I)>The axis is along the tangential direction of the i-th follower underwater vehicle track (i.e. the direction of the resultant speed of the i-th follower underwater vehicle), the axis is along the tangential direction of the i-th follower underwater vehicle track (i.e. the direction of the resultant speed of the i-th follower underwater vehicle track)>Horizontal plane x with axis parallel to the geodetic coordinate system _I Oy _I ，/>The axis is determined by the right hand spiral rule;

navigation coordinate system: the coordinate origin coincides with a virtual point S corresponding to the ith follower underwater vehicle on the navigation track of the ith follower underwater vehicle, the horizontal axis is along the tangential direction of the track at the virtual point Q, and the vertical axis is parallel to the horizontal plane x of the geodetic coordinate system _I Oy _I The vertical axis is determined by the right hand spiral law.

FIG. 3 is a geodetic coordinate system { I }, a carrier coordinate system { B }, a _i Coordinate system of combined velocity { W } _i Schematic of { and navigator coordinate systems { F }; symbol M for pilot coordinate system { F }, in the figure _F -x _F y _F z _F The navigation track of the pilot underwater vehicle is the expected navigation track. The underwater vehicle is shown as AUV. The invention is only focused on the i-th follower underwater vehicleSpeed control in the axial direction, therefore, the combined speed coordinate system { W _i [ only ]>Shaft (S)>Shaft and->The shaft is omitted. In addition, the invention does not consider the influence of the non-zero variable attack angle and sideslip angle of the pilot underwater vehicle on the formation tracking, so that the pilot coordinate system { F } is completely coincident with the combined speed coordinate system of the pilot underwater vehicle.

First, based on the assumption that the pitch angle of the unmanned underwater vehicle is limited and the roll angle and the roll angular velocity are all about zero, the kinematic model of the i-th unmanned underwater vehicle can be simplified to:

in the above, the position coordinate of the ith follower underwater vehicle under the geodetic coordinate system is p _i ＝[x _i ，y _i ，z _i ] ^T ,θ _i For pitch angle of the i-th follower underwater vehicle, i.e. carrier coordinate system of the i-th follower underwater vehicle { B _i Is wound aroundThe axis rotates to a rotation angle phi when parallel to the earth coordinate system { I }, phi _i For the heading angle of the i-th follower underwater vehicle, i.e. the carrier coordinate system { B of the i-th follower underwater vehicle _i Winding->The axis rotates to a rotation angle parallel to the earth coordinate system { I }, q _i For the ith follower underwater vehicle in the carrier coordinate system { B ] _i Pitch speed under }; r is (r) _i For the ith follower underwater vehicle in the carrier coordinate system { B ] _i The angular velocity of the heading at }, u _i 、v _i And w _i The ith follower underwater vehicle respectively along the carrier coordinate system { B _i }>Shaft(s)>Shaft and->The translational speed of the shaft, the points on the parameter represent deriving the parameter.

Then, taking the influence of the non-zero variable attack angle and sideslip angle of the ith follower underwater vehicle on the formation tracking into consideration, and constructing an ith follower underwater vehicle formation tracking error dynamics model;

the formation control aims at enabling the ith follower underwater vehicle to always coincide with the virtual point S, which is equivalent to enabling a virtual point C corresponding to the ith follower underwater vehicle and the ith follower underwater vehicle to maintain a desired control distance d on the navigation track of the pilot underwater vehicle _i ＝[d _xi ，d _yi ，d _zi ] ^T The method comprises the steps of carrying out a first treatment on the surface of the The desired control pitch between the virtual point C and the ith follower underwater vehicle may be determined based on the desired time-varying formation.

Thus, the formation tracking error e of the ith follower underwater vehicle in the navigation coordinate system _pi The expression of (2) is as follows:

in the above formula, the position coordinate of the virtual point C in the geodetic coordinate system is p _0* ＝[x _0* ，y _0* ，z _0* ] ^T ，Rotation matrix from geodetic coordinate system to navigational coordinate system, >

Wherein the matrix is rotatedSatisfy->Then->The method comprises the following steps: />

In the above formula, χ _0* For the track angle of said virtual point C, i.e. the angle of rotation when the navigational coordinate system is rotated about its vertical axis to be parallel to the geodetic coordinate system { I }, v _0* Is the angle of latency of the virtual point C, i.e. the angle of rotation when the navigational coordinate system is rotated about its longitudinal axis to be parallel to the geodetic coordinate system I.

e _pi The time derivative is as follows:

in the above, ψ _ei For the error rotation angle corresponding to the heading angle of the ith follower underwater vehicle, namely the carrier coordinate system { B _i Is wound aroundThe axis rotates to a rotation angle theta parallel to the navigation coordinate system _ei For the error rotation angle corresponding to the pitch angle of the ith follower underwater vehicle, namely the carrier coordinate system { B of the ith follower underwater vehicle _i Winding->The axis rotates to a rotation angle parallel to the navigation coordinate system, < >>Is a rotation matrix from the geodetic coordinate system { I } to the navigational coordinate system,/and }>Carrier coordinate system { B for the ith follower underwater vehicle _i Rotation matrix of { I } to geodetic coordinate system, v _0* For the sum velocity vector of the virtual point C in the navigation coordinate system, v _Bi Representing the ith follower underwater vehicle in its carrier coordinate system { B _i Velocity vector, v at } _wi Represent the firsti number of follower underwater vehicles in their coordinate system { W } _i A resultant velocity vector under.

In the derivation process, nonlinear interference generated by tracking control of three-dimensional time-varying formation by using non-zero attack angle and sideslip angle of ith follower underwater vehicle under navigation coordinate systemKinematic uncertainty term f representing the ith follower underwater vehicle _pi ；

in the above, f _pi ＝[f _xi ,f _yi ,f _zi ] ^T ，f _xi 、f _yi And f _zi Respectively f _pi Transverse, longitudinal and vertical axis components, U _i For the resultant speed of the ith follower underwater vehicle in its carrier coordinate system, U _0* Is the resultant velocity of the virtual point C in the geodetic coordinate system.

The invention obtains the expression of the uncertain item of the kinematics of the ith follower underwater vehicle by simplifying and deducing the formation tracking error dynamics model of the ith follower underwater vehicle, and lays a foundation for the compensation of the uncertain item of the follow-up kinematics.

Then, based on the uncertain items of the kinematics of the ith follower underwater vehicle, designing an improved line-of-sight guidance law, which specifically comprises:

the structures are respectively used for counteracting f _yi And f _zi Is a first auxiliary variable delta of (1) _yi And a second auxiliary variable delta _zi ；δ _yi And delta _zi The following equation is satisfied:

unfolding the delta _yi And delta _zi Expression of (2), yields:

in the above-mentioned method, the step of,γ _zi ＝f _zi /U _i ，Δ _yi >0，Δ _zi >0，/>Δ _yi for a first line-of-sight distance, delta, of an ith follower underwater vehicle _zi A second line of sight distance for the ith follower underwater vehicle.

Constructing a desired line of sight angle based on the first auxiliary variable and the second auxiliary variable; the expected sight angle comprises an expected error rotation angle corresponding to the heading angle of the i-th follower underwater vehicle and an expected error rotation angle corresponding to the pitch angle of the i-th follower underwater vehicle; the expression of the desired line of sight angle is:

wherein, the liquid crystal display device comprises a liquid crystal display device,underwater navigation for the ith followerDesired error rotation angle corresponding to pitch angle of the walker, < >>And (3) the expected error rotation angle corresponding to the heading angle of the i-th follower underwater vehicle.

Constructing an expected pitch angle and an expected heading angle of the ith follower underwater vehicle based on a coordinate conversion equivalence principle and the expected line-of-sight angle;

namely: to make the error rotation angle (psi) _ei ，θ _ei ) Respectively towards the angle of rotation of the expected errorAccording to the principle of coordinate transformation equivalence->And combining corresponding rotation matrix calculation to obtain the following steps:

in the above-mentioned method, the step of,for the desired pitch angle of the ith follower underwater vehicle,/for the i-th follower underwater vehicle>For the desired heading angle of the ith follower underwater vehicle, χ _di For the i-th follower underwater vehicle's desired track angle, v _di Is the desired potential angle of the ith follower underwater vehicle.

Here the number of the elements is the number,

the invention skillfully converts the compensation of the uncertain item of the kinematics of the i-th follower underwater vehicle into the control of the attitude angle of the i-th follower underwater vehicle, thereby enabling the controller to realize the compensation target more easily.

And finally, on the basis of the ith follower underwater vehicle formation tracking error dynamics model and the sight guidance law, designing a dynamics controller for guiding the pitch angle and the heading angle of the ith follower underwater vehicle to reach expected values.

The expression of the kinetic controller is as follows:

in the above, u _di For the expected value of the voyage speed of the ith follower underwater vehicle along the transverse axis of the carrier coordinate system of the ith follower underwater vehicle, q _di Is the expected value of the pitch angle speed of the ith follower underwater vehicle in the carrier coordinate system, r _di Is the expected value of the heading angular velocity of the ith follower underwater vehicle in the carrier coordinate system, alpha _i Angle of attack, beta, for the ith follower underwater vehicle _i Sideslip angle, k, for the ith follower underwater vehicle _di For the gain of the first controller,for the second controller gain, k _ψi Gain for the third controller.

FIG. 4 is a block diagram of a three-dimensional spatial multi-underwater vehicle formation control that benefits from compensation of the i-th follower underwater vehicle's kinematics uncertainty term by a kinematic controller that is significantly more robust than existing controllers in terms of its anti-jamming capability and its time-varying formation tracking capability.

Specifically, in S23, the kinetic parameters include, but are not limited to: center of gravity position coordinates, pitch angle, heading angle, and speed of travel along the horizontal, vertical and vertical axes of its carrier coordinate system.

In practical use, the kinetic parameters may be acquired by sensors onboard the underwater vehicle.

Specifically, the step S23 includes:

positioning corresponding virtual points on the navigation track of each follower underwater vehicle based on navigation time;

recording the deviation between the position coordinates of each follower underwater vehicle at the navigation time and the position coordinates of the virtual point as a third deviation;

and taking the product of the third deviation and a rotation matrix from the geodetic coordinate system to the navigation coordinate system as a formation tracking error of each follower underwater vehicle.

It is understood that the formula of S23 is as follows

Wherein p is _c Is the position coordinates of a virtual point located on the navigation track of the i-th follower underwater vehicle.

The invention provides a calculation mode of the formation tracking error of each follower underwater vehicle during application, and provides a basis for formation control of each follower underwater vehicle.

Specifically, the kinematics uncertainty term of the ith follower underwater vehicle in S24 may be according to the foregoing formulaAnd (5) calculating.

Specifically, the step S25 includes:

the S25.1: performing formation tracking control based on the formation tracking error, the kinematic uncertainty item, and a dynamics controller;

the dynamics controller is designed based on a formation tracking error dynamics model and a line-of-sight guidance law; the formation tracking error dynamics model is constructed on the basis of considering the kinematics uncertainty item; the line-of-sight guidance law is designed with the aim of compensating for the kinematic uncertainty term.

Further, S25.1 includes:

determining an angle of attack and a sideslip angle for each of said follower underwater vehicles;

determining a track angle and a hidden angle of the virtual point;

calculating the total speed of each follower underwater vehicle under the carrier coordinate system according to the navigation speed of each follower underwater vehicle along the transverse axis, the longitudinal axis and the vertical axis of the carrier coordinate system;

substituting each follower underwater vehicle formation tracking error, a kinematic uncertainty item, an attack angle, a sideslip angle, a longitudinal inclination angle, a heading angle and a combination speed under a carrier coordinate system into the dynamics controller to obtain an output value;

and carrying out formation tracking control according to the output value.

The method comprises the steps of regarding the tracking control influence of a non-zero variable attack angle and a sideslip angle generated by the interference of ocean currents on each follower underwater vehicle on a time-varying formation of each follower underwater vehicle as a kinematic uncertainty item of each follower underwater vehicle, and constructing a formation tracking error dynamics model; and then designing a line-of-sight guidance law by taking the compensated kinematics uncertainty item as a target, and finally designing a dynamics controller on the basis of the formation tracking error dynamics model and the line-of-sight guidance law. When a dynamics controller is used for the formation tracking control of each of the follower underwater vehicles, the time-varying formation tracking capability and the disturbance rejection capability of each of the follower underwater vehicles are remarkably improved thanks to the compensation of the kinematics uncertainty item.

In addition, the formation type can be adjusted in real time according to the requirements, and the task requirements of complex and severe environments are met. And under the conditions of avoiding the excessive complexity of a three-dimensional formation control algorithm and the excessive high calculation cost, the time-varying track tracking control of the formation of multiple underwater vehicles is realized, the defect of inspection of a single underwater vehicle is overcome, and the inspection efficiency and the detection precision are improved.

In order to verify the effectiveness of the three-dimensional multi-underwater vehicle formation tracking control algorithm, simulation test and analysis are carried out. The specific parameters are as follows:

the motion equation of the pilot underwater vehicle is thatThe motion equation of the follower underwater vehicle is +.>u _i (0) =0; the initial positions of the follower underwater vehicle are respectively as follows: p is p ₁ (0)＝[0,0,0] ^T ,p ₂ (0)＝[10,-10,-3] ^T ,p ₃ (0)＝[20,-10,-3] ^T ,p ₄ (0)＝[-10,-10,-3] ^T The method comprises the steps of carrying out a first treatment on the surface of the The required time-varying relative positions between the following underwater vehicle and the pilot underwater vehicle are d ₁ ＝[10sin(πt/600),0,0] ^T ,/> The parameters are selected as follows: delta _y ＝5，k _x ＝k _θ ＝k _ψ =0.3, t=300 s, the remaining motion parameters are zero.

After the parameters are given, the track, the speed and the formation of the multi-underwater vehicle in the solving time are calculated, the change curves of the track, the formation and the speed of the underwater vehicle in the time period and the dynamic errors are output, and a three-dimensional motion track simulation diagram of the formation of the underwater vehicle shown in fig. 5 and an error schematic diagram of the formation track of the underwater vehicle shown in fig. 6 (a) to 6 (c) are obtained. Fig. 5 shows the actual trajectory of the pilot and four follower underwater vehicles and their formations of 0 seconds, 150 seconds and 300 seconds in 3D space, from which it can be seen that the time-varying formations formed by the pilot and four followers have been well established, the formations of the four follower underwater vehicles gradually changing from triangular to diamond. Fig. 6 (a) -6 (c) show the formation errors of the underwater vehicles, and it can be seen that they reach and remain within the prescribed error limits, i.e. the formation of the underwater vehicle is guaranteed.

In a third aspect, the present invention provides an underwater vehicle formation tracking control device, where the following description refers to an underwater vehicle formation tracking control device and an underwater vehicle formation tracking control method applicable to a follower underwater vehicle.

Fig. 7 illustrates a schematic structure of an underwater vehicle formation tracking control apparatus adapted for a follower underwater vehicle, as shown in fig. 7, the apparatus comprising:

a first obtaining module 31, configured to obtain a navigation track and a desired time-varying formation;

an expected control pitch determination module 32 for determining an expected control pitch with the navigation track based on the expected time-varying formation;

a first formation tracking error determination module 33, configured to determine a formation tracking error according to the expected control pitch, the dynamics parameters of the navigation track, and the dynamics parameters of the follower underwater vehicle;

a first kinematic uncertainty term determination module 34 for determining a kinematic uncertainty term based on non-linear disturbances generated by non-zero angle of attack and sideslip angle and the desired control pitch;

a first formation tracking control module 35 for performing formation tracking control based on the kinematic uncertainty item and the formation tracking error.

The invention provides a formation tracking control device of an underwater vehicle, which is suitable for a follower underwater vehicle and comprises the following components: acquiring a navigation track and an expected time-varying formation; the navigation trajectory is a navigation trajectory of a pilot underwater vehicle associated with the follower underwater vehicle; determining an expected control interval between the navigation track and the target control interval according to the expected time-varying formation; determining a formation tracking error according to the expected control interval, the dynamic parameters of the navigation track and the dynamic parameters of the underwater vehicle of the follower; determining a kinematic uncertainty term from nonlinear disturbances generated by non-zero angle of attack and sideslip angle and the desired control pitch; and performing formation tracking control based on the kinematics uncertainty item and the formation tracking error. When the follower underwater vehicle carries out formation tracking control on the follower underwater vehicle, the invention compensates the uncertain item of the kinematics of the follower underwater vehicle, improves the disturbance rejection capability of the follower underwater vehicle, and further enhances the formation tracking capability of the follower underwater vehicle. In addition, the invention introduces the expected time-varying formation, thereby being capable of adjusting the formation type in real time according to the requirement and meeting the task requirement of complex and severe environment.

Embodiments of the underwater vehicle formation tracking control device suitable for a follower underwater vehicle correspond to embodiments of the underwater vehicle formation tracking control method suitable for a follower underwater vehicle, and are not described in detail herein.

In a fourth aspect, the present invention provides an underwater vehicle formation tracking control device, where the following description refers to an underwater vehicle formation tracking control device and an underwater vehicle formation tracking control method applicable to a pilot underwater vehicle.

Fig. 8 illustrates a schematic structure of an underwater vehicle formation tracking control apparatus suitable for a pilot underwater vehicle, as shown in fig. 8, the apparatus comprising:

a second acquisition module 41 for acquiring a navigation track and a desired time-varying formation;

a navigation track determining module 42, configured to determine a navigation track of each follower underwater vehicle according to the navigation track and the desired time-varying formation;

a second formation tracking error determination module 43, configured to determine a formation tracking error of each of the follower underwater vehicles according to the navigation track and a kinetic parameter of each of the follower underwater vehicles;

a second kinematic uncertainty term determination module 44 for determining a kinematic uncertainty term for each of the follower underwater vehicles based on the received non-linear disturbances generated by the non-zero angle of attack and sideslip angle of each of the follower underwater vehicles;

A second formation tracking control module 45 for performing formation tracking control for each of the follower underwater vehicles based on the kinematic uncertainty item and the formation tracking error.

The invention provides a formation tracking control device of an underwater vehicle, which is suitable for a pilot underwater vehicle and comprises the following components: acquiring a navigation track and an expected time-varying formation; determining the navigation track of each follower underwater vehicle according to the navigation track and the expected time-varying formation; determining a formation tracking error of each follower underwater vehicle according to the navigation track and the dynamic parameters of each follower underwater vehicle; determining a kinematic uncertainty term of each of the follower underwater vehicles according to the received nonlinear interference generated by the non-zero attack angle and sideslip angle of each of the follower underwater vehicles; and performing formation tracking control on each follower underwater vehicle based on the kinematics uncertainty item and the formation tracking error. When the pilot underwater vehicle carries out formation tracking control on the follower underwater vehicle, the invention compensates the uncertain item of the kinematics of the follower underwater vehicle, improves the disturbance rejection capability of the follower underwater vehicle, and further enhances the formation tracking capability of the follower underwater vehicle. In addition, the invention introduces the expected time-varying formation, thereby being capable of adjusting the formation type in real time according to the requirement and meeting the task requirement of complex and severe environment.

The formation tracking capability of the underwater vehicle is remarkably improved.

In a sixth aspect, the present invention also provides a computer program product comprising a computer program, the computer program being storable on a non-transitory computer readable storage medium, the computer program being capable of performing the method of formation tracking control of an underwater vehicle according to the first aspect or the method of formation tracking control of an underwater vehicle according to the second aspect, when the computer program is executed by a processor.

In a seventh aspect, the present invention also provides a non-transitory computer readable storage medium having stored thereon a computer program which, when executed by a processor, is implemented to perform the method of formation tracking control of an underwater vehicle described in the first aspect above or to perform the method of formation tracking control of an underwater vehicle described in the second aspect above.

The apparatus embodiments described above are merely illustrative, wherein the elements illustrated as separate elements may or may not be physically separate, and the elements shown as elements may or may not be physical elements, may be located in one place, or may be distributed over a plurality of network elements. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of this embodiment. Those of ordinary skill in the art will understand and implement the present invention without undue burden.

From the above description of the embodiments, it will be apparent to those skilled in the art that the embodiments may be implemented by means of software plus necessary general hardware platforms, or of course may be implemented by means of hardware. Based on this understanding, the foregoing technical solution may be embodied essentially or in a part contributing to the prior art in the form of a software product, which may be stored in a computer readable storage medium, such as ROM/RAM, a magnetic disk, an optical disk, etc., including several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute the method described in the respective embodiments or some parts of the embodiments.

Finally, it should be noted that: the above embodiments are only for illustrating the technical solution of the present invention, and are not limiting; although the invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. An underwater vehicle formation tracking control method suitable for a follower underwater vehicle, the method comprising:

acquiring a navigation track and an expected time-varying formation;

performing formation tracking control based on the kinematic uncertainty item and the formation tracking error;

The determining a formation tracking error according to the expected control interval, the dynamic parameter of the navigation track and the dynamic parameter of the follower underwater vehicle comprises the following steps:

taking the product of the second deviation and a rotation matrix from the geodetic coordinate system to the navigation coordinate system as a formation tracking error;

the expression of the kinematics uncertainty term of the follower underwater vehicle is as follows:

in the above, f _p For the kinematic uncertainty term of the follower underwater vehicle, f _x 、f _y And f _z Respectively f _p D is the expected control distance, v, between the follower underwater vehicle and the navigation track _B Representing the velocity vector, v of the follower underwater vehicle in its carrier coordinate system _w Representing a resultant velocity vector of the follower underwater vehicle in its resultant velocity coordinate system, Rotation matrix from geodetic coordinate system to navigational coordinate system,>and T is a transposed symbol for a rotation matrix from a carrier coordinate system to a geodetic coordinate system of the follower underwater vehicle.

2. The underwater vehicle formation tracking control method of claim 1, wherein the kinetic parameters include, but are not limited to: center of gravity position coordinates, pitch angle, heading angle, and speed of travel along the horizontal, vertical and vertical axes of its carrier coordinate system.

3. The underwater vehicle formation tracking control method according to claim 2, characterized in that the formation tracking control based on the kinematic uncertainty item and the formation tracking error includes:

performing formation tracking control based on the formation tracking error, the kinematic uncertainty item, and a dynamics controller;

4. An underwater vehicle formation tracking control method as claimed in claim 3, characterized in that the formation tracking control based on the formation tracking error, the kinematic uncertainty term and a dynamics controller includes:

Determining an attack angle and a sideslip angle;

determining a track angle and a hidden angle of the virtual point;

substituting the formation tracking error, the kinematic uncertainty item, the attack angle, the sideslip angle, the pitch angle, the heading angle and the combined speed under the carrier coordinate system, and the latency angle of the virtual point, the track angle and the combined speed of the virtual point into the dynamics controller to obtain an output value;

and carrying out formation tracking control according to the output value.

5. An underwater vehicle formation tracking control method as claimed in claim 3, characterized in that the line of sight guidance law is designed with the purpose of compensating the kinematic uncertainty item, comprising:

constructing first and second auxiliary variables for canceling the longitudinal and vertical axis components of the kinematically uncertainty item, respectively;

constructing a desired line of sight angle based on the first auxiliary variable and the second auxiliary variable; the expected sight angle comprises an expected error rotation angle corresponding to the heading angle of the follower underwater vehicle and an expected error rotation angle corresponding to the pitch angle of the follower underwater vehicle;

And constructing the expected pitch angle and the expected heading angle of the follower underwater vehicle based on the coordinate conversion equivalence principle and the expected line-of-sight angle.

6. The underwater vehicle formation tracking control method of claim 5, wherein the dynamics controller is designed based on a formation tracking error dynamics model and a line-of-sight guidance law, comprising:

and generating the follower underwater vehicle dynamics controller by taking the longitudinal inclination angle of the follower underwater vehicle tending to the expected longitudinal inclination angle and the heading angle of the follower underwater vehicle tending to the expected heading angle as targets and combining the follower underwater vehicle formation tracking error dynamics model.

7. An underwater vehicle formation tracking control method suitable for a pilot underwater vehicle, comprising:

acquiring a navigation track and an expected time-varying formation;

performing formation tracking control for each of the follower underwater vehicles based on the kinematic uncertainty term and the formation tracking error;

the step of determining the formation tracking error of each follower underwater vehicle according to the navigation track of each follower underwater vehicle and the received dynamic parameters of each follower underwater vehicle comprises the following steps:

taking the product of the third deviation and a rotation matrix from a geodetic coordinate system to a navigation coordinate system as a formation tracking error of each follower underwater vehicle;

the expression of the kinematics uncertainty term of each follower underwater vehicle is as follows:

in the above, f _pi For the i-th follower underwater vehicle, f _xi 、f _yi And f _zi Respectively f _pi Is a transverse axis component, a longitudinal axis component and a vertical axis component, v _Bi Representing the velocity vector, v of the ith follower underwater vehicle in its carrier coordinate system _wi Representing the resultant velocity vector of the ith follower underwater vehicle in its resultant velocity coordinate system,rotation matrix from geodetic coordinate system to navigational coordinate system,>the rotation matrix from the carrier coordinate system of the ith follower underwater vehicle to the geodetic coordinate system is represented by T, which is the transposed symbol.

8. An underwater vehicle formation tracking control device adapted for use with a follower underwater vehicle, the device comprising:

The first formation tracking control module is used for performing formation tracking control based on the kinematics uncertainty item and the formation tracking error;

in the above, f _p For the kinematic uncertainty term of the follower underwater vehicle, f _x F and f _z Respectively f _p D is the expected control distance, v, between the follower underwater vehicle and the navigation track _B Representing the velocity vector, v of the follower underwater vehicle in its carrier coordinate system _w Representing a resultant velocity vector of the follower underwater vehicle in its resultant velocity coordinate system,rotation matrix from geodetic coordinate system to navigational coordinate system,>and T is a transposed symbol for a rotation matrix from a carrier coordinate system to a geodetic coordinate system of the follower underwater vehicle.

9. An underwater vehicle formation tracking control device suitable for a pilot underwater vehicle, the device comprising:

the second formation tracking error determining module is used for determining the formation tracking error of each follower underwater vehicle according to the navigation track and the dynamic parameters of each follower underwater vehicle;

a second formation tracking control module for performing formation tracking control on each of the follower underwater vehicles based on the kinematic uncertainty item and the formation tracking error;

in the above, f _pi For the i-th follower underwater vehicle, f _xi 、f _yi And f _zi Respectively f _pi Is a transverse axis component, a longitudinal axis component and a vertical axis component, v _Bi Representing the velocity vector, v of the ith follower underwater vehicle in its carrier coordinate system _wi Representing the resultant velocity vector of the ith follower underwater vehicle in its resultant velocity coordinate system,rotation matrix from geodetic coordinate system to navigational coordinate system, >The rotation matrix from the carrier coordinate system of the ith follower underwater vehicle to the geodetic coordinate system is represented by T, which is the transposed symbol.

10. An underwater vehicle comprising an underwater vehicle formation tracking control device as claimed in claim 8 or claim 9.