Nothing Special   »   [go: up one dir, main page]

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

Underwater vehicle and formation tracking control method and device thereof Download PDF

Info

Publication number
CN116339355B
CN116339355B CN202310205875.4A CN202310205875A CN116339355B CN 116339355 B CN116339355 B CN 116339355B CN 202310205875 A CN202310205875 A CN 202310205875A CN 116339355 B CN116339355 B CN 116339355B
Authority
CN
China
Prior art keywords
underwater vehicle
follower
formation
coordinate system
angle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202310205875.4A
Other languages
Chinese (zh)
Other versions
CN116339355A (en
Inventor
高宏宇
张木
薛任宇欣
张育玮
孔凡忠
王锐
赵宏亮
黄磊
刘鹏
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Xinxing Jihua Beijing Intelligent Equipment Technology Research Institute Co ltd
Original Assignee
Xinxing Jihua Beijing Intelligent Equipment Technology Research Institute Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Xinxing Jihua Beijing Intelligent Equipment Technology Research Institute Co ltd filed Critical Xinxing Jihua Beijing Intelligent Equipment Technology Research Institute Co ltd
Priority to CN202310205875.4A priority Critical patent/CN116339355B/en
Publication of CN116339355A publication Critical patent/CN116339355A/en
Application granted granted Critical
Publication of CN116339355B publication Critical patent/CN116339355B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/04Control of altitude or depth
    • G05D1/06Rate of change of altitude or depth
    • G05D1/0692Rate of change of altitude or depth specially adapted for under-water vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

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 0 =[x 0 ,y 0 ,z 0 ] TRotation 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 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 0 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 0 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 0 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,,
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,,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.
The navigation track and the expected time-varying formation engineer are designed according to the field working conditions and engineering experience;
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.
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 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 IThe 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.
(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 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 ] Ti 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* ] TRotation 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
Thus, the first and second substrates are bonded together,can be further developed as:
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:
Wherein,,
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,,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.
It should be noted that the kinematic controller is a programming program that encapsulates the control algorithm.
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;
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 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 1 (0)=[0,0,0] T ,p 2 (0)=[10,-10,-3] T ,p 3 (0)=[20,-10,-3] T ,p 4 (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 1 =[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.
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 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 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 (10)

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;
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;
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:
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;
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;
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.
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;
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 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;
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;
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:
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;
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 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;
The first formation tracking control module is used for performing formation tracking control based on the kinematics 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:
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;
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 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 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 and the 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;
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;
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:
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;
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.
10. An underwater vehicle comprising an underwater vehicle formation tracking control device as claimed in claim 8 or claim 9.
CN202310205875.4A 2023-03-03 2023-03-03 Underwater vehicle and formation tracking control method and device thereof Active CN116339355B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310205875.4A CN116339355B (en) 2023-03-03 2023-03-03 Underwater vehicle and formation tracking control method and device thereof

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310205875.4A CN116339355B (en) 2023-03-03 2023-03-03 Underwater vehicle and formation tracking control method and device thereof

Publications (2)

Publication Number Publication Date
CN116339355A CN116339355A (en) 2023-06-27
CN116339355B true CN116339355B (en) 2023-10-20

Family

ID=86890798

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310205875.4A Active CN116339355B (en) 2023-03-03 2023-03-03 Underwater vehicle and formation tracking control method and device thereof

Country Status (1)

Country Link
CN (1) CN116339355B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117193305B (en) * 2023-09-21 2024-06-07 中兵智能创新研究院有限公司 Autonomous following perception tracking method and device for complex sea-state amphibious aircraft

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104076688A (en) * 2014-07-17 2014-10-01 山东省科学院海洋仪器仪表研究所 Master-slave type cooperative control method for autonomous underwater vehicles
CN105978725A (en) * 2016-05-13 2016-09-28 芦慧 Non-fragile distributed fault estimation method based on sensor network
CN108303891A (en) * 2018-02-11 2018-07-20 浙江大学 More AUV distributed collaborations tracking and controlling methods under being disturbed based on uncertain ocean current
CN108549394A (en) * 2018-04-12 2018-09-18 哈尔滨工程大学 A kind of more AUV straight lines formation control methods based on pilotage people and virtual pilotage people
CN110989639A (en) * 2019-12-31 2020-04-10 中国电子科技集团公司信息科学研究院 Underwater vehicle formation control method based on stress matrix
CN112947505A (en) * 2021-03-22 2021-06-11 哈尔滨工程大学 Multi-AUV formation distributed control method based on reinforcement learning algorithm and unknown disturbance observer

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104076688A (en) * 2014-07-17 2014-10-01 山东省科学院海洋仪器仪表研究所 Master-slave type cooperative control method for autonomous underwater vehicles
CN105978725A (en) * 2016-05-13 2016-09-28 芦慧 Non-fragile distributed fault estimation method based on sensor network
CN108303891A (en) * 2018-02-11 2018-07-20 浙江大学 More AUV distributed collaborations tracking and controlling methods under being disturbed based on uncertain ocean current
CN108549394A (en) * 2018-04-12 2018-09-18 哈尔滨工程大学 A kind of more AUV straight lines formation control methods based on pilotage people and virtual pilotage people
CN110989639A (en) * 2019-12-31 2020-04-10 中国电子科技集团公司信息科学研究院 Underwater vehicle formation control method based on stress matrix
CN112947505A (en) * 2021-03-22 2021-06-11 哈尔滨工程大学 Multi-AUV formation distributed control method based on reinforcement learning algorithm and unknown disturbance observer

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Formation Tracking Control for A Team of Networked Underwater Robot Systems with Uncertain Hydrodynamics;Feng, Z等;IEEE International Conference on Real-time Computing and Robotics (IEEE RCAR);全文 *
Observer-Based Neural Formation Control of Leader–Follower AUVs With Input Saturation;Jinqiang Wang等;IEEE Systems Journal;第第15卷卷(第第2期期);全文 *
基于动态φ值的领航跟随法多机器人编队控制;赵明,林茂松,黄玉清;西南科技大学学报;第28卷(第4期);全文 *

Also Published As

Publication number Publication date
CN116339355A (en) 2023-06-27

Similar Documents

Publication Publication Date Title
CN107168312B (en) Space trajectory tracking control method for compensating UUV kinematic and dynamic interference
CN105955268B (en) A kind of UUV moving-target sliding mode tracking control methods considering Local obstacle avoidance
Roberts et al. Advances in unmanned marine vehicles
CN108829099A (en) A kind of drive lacking unmanned boat trace tracking method based on the control of limited Backstepping
Li et al. Intelligent PID guidance control for AUV path tracking
Wang et al. A simultaneous planning and control method integrating APF and MPC to solve autonomous navigation for USVs in unknown environments
CN105843224A (en) AUV horizontal planar path tracking control method based on neural dynamic model and backstepping method
CN114115262B (en) Multi-AUV actuator saturation cooperative formation control system and method based on azimuth information
Zhang et al. Adaptive neural network control for visual docking of an autonomous underwater vehicle using command filtered backstepping
CN116339355B (en) Underwater vehicle and formation tracking control method and device thereof
Zhuang et al. Motion control and collision avoidance algorithms for unmanned surface vehicle swarm in practical maritime environment
He et al. Robust orientation-sensitive trajectory tracking of underactuated autonomous underwater vehicles
CN114967714A (en) Anti-interference motion control method and system for autonomous underwater robot
Ohrem et al. Robust adaptive backstepping DP control of ROVs
Esteba et al. Docking of non-holonomic AUVs in presence of ocean currents: A comparative survey
CN109885096A (en) A kind of autonomous underwater robot path trace closed loop control method based on Lyapunov-MPC technology
CN111798702A (en) Unmanned ship path tracking control method, system, storage medium and terminal
CN115480580A (en) NMPC-based underwater robot path tracking and obstacle avoidance control method
Lin et al. The fuzzy-based visual intelligent guidance system of an autonomous underwater vehicle: realization of identifying and tracking underwater target objects
CN113296505B (en) Unmanned ship multi-mode path tracking control method based on speed change LOS
Du et al. Research on UUV recovery active disturbance rejection control based on LMNN compensation
CN114063624A (en) Multi-mode planning motion controller of crawling unmanned submersible and control method thereof
CN117472061B (en) Unmanned ship formation control design method with limited time and stable preset performance
de Cerqueira Gava et al. AUV cruising auto pilot for a long straight confined underwater tunnel
Kokegei et al. Fully coupled 6 degree-of-freedom control of an over-actuated autonomous underwater vehicle

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant