Underwater vehicle formation control method and system based on LOS constraints

Abstract

The present invention relates to a method and system for underwater vehicle formation control based on LOS constraint, comprising the following steps: S1: establishing a mathematical model for underwater vehicle formation control; S2: defining the tracking error of the formation based on the mathematical model of the underwater vehicle, and designing an obstacle Lyapunov function to process the line-of-sight constraint; S3: designing an adaptive integral guidance strategy with LOS constraint under local communication under the premise of satisfying connectivity and safety; S4: constructing a finite time observer, fitting the disturbance variables in the model, and using the fitted approximation to compensate the control input of the system; S5: designing a constrained formation controller based on an adaptive integral guidance strategy hybrid finite time observer to realize underwater vehicle formation control with LOS constraint under local communication. The present invention effectively improves the accuracy and reliability of formation control and enhances the robustness of the system.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle formation control, and in particular to an underwater vehicle formation control method and system based on LOS constraints. Background Art

[0002] In the field of surface vehicle formation control, effective communication between formations is the key to achieving collaborative operations. Although traditional radio communication methods are widely used due to their long transmission distance and wide coverage, their fault tolerance and reliability face many challenges in actual application. Especially in complex and changeable marine environments, radio signals are susceptible to interference, resulting in reduced communication quality and even possible communication interruption, which poses a threat to the formation control and collaborative operations of surface vehicles.

[0003] In view of this, researchers began to seek more robust communication methods to enhance the reliability and stability of surface vehicle formation control. As an important means of information transmission, visual sensors have attracted much attention due to their intuitive, accurate, and real-time characteristics. Through visual sensors, surface vehicles can obtain image information of the surrounding environment in real time, thereby realizing the transmission and sharing of status information. However, most of the current research work focuses on improving the accuracy and stability of tracking control, while less attention is paid to the constraints of the sensor's field of view angle. In practical applications, the limitation of the sensor's field of view angle may lead to incomplete or distorted information transmission, which in turn affects the accuracy and reliability of formation control.

[0004] In addition, when the surface vehicles are in formation, they also need to consider the relative position and angle relationship between the vehicles, as well as the impact of environmental interference and other factors on formation control. How to achieve formation control of underwater vehicles with LOS constraints under local communication, and how to achieve distributed formation control of surface vehicles are important issues facing current research. Summary of the invention

[0005] In order to solve the above problems, the purpose of the present invention is to provide an underwater vehicle formation control method and system based on LOS constraints, so as to realize distributed formation control of surface vehicles under local communication and limited perception conditions, while considering the constraints of sensor field of view angle to improve the stability and accuracy of formation control.

[0006] To achieve the above object, the present invention adopts the following technical solutions:

[0007] A method for controlling an underwater vehicle formation based on LOS constraints comprises the following steps:

[0008] S1: Establish a mathematical model for underwater vehicle formation control;

[0009] S2: Based on the mathematical model of underwater vehicles, the tracking error of the formation is defined, and the obstacle Lyapunov function is designed to handle the line-of-sight constraint to ensure the connectivity and safety of underwater vehicles;

[0010] S3: Design an adaptive integral guidance strategy with LOS constraints under local communication while satisfying connectivity and security.

[0011] S4: Construct a finite-time observer to fit the disturbance variables in the model and use the fitted approximation to compensate the control input of the system to suppress environmental disturbances and improve the tracking accuracy of the robot;

[0012] S5: Design a constrained formation controller based on a hybrid finite-time observer with an adaptive integral guidance strategy to achieve underwater vehicle formation control with LOS constraints under local communication.

[0013] Furthermore, the mathematical model of underwater vehicle formation control is a mathematical model of horizontal plane motion considering the three degrees of freedom of forward, lateral and bow:

[0014] The underwater vehicle fleet includes underwater vehicles, numbered from 0 to ; Underwater vehicle As the leader, leading the overall fleet movement, the rest of the underwater vehicles are automatically controlled For followers, among them ;No. The dynamic equation of an underwater vehicle is as follows:

[0015]

[0016] in, For the An underwater vehicle in an earth-fixed coordinate system The centroid coordinates of and Respectively The underwater vehicle is in the Earth-fixed coordinate system and The velocity component in the direction, For the The yaw angle of the underwater vehicle, For the The rate of change of the yaw angle of an underwater vehicle over time, , ,and Respectively represent The forward, lateral and yaw velocities of the underwater vehicle are calculated by using the Lagrangian mechanics equations to add perturbations to the inertial parameters. and the hydrodynamic damping coefficient ;

[0017] Therefore, the dynamic model of the underwater vehicle is:

[0018]

[0019] In the formula, , and Respectively represent The forward, lateral and yaw velocity change rates of the underwater vehicle, and Respectively represent The forward moment and yaw moment of an underwater vehicle, is the inertia parameter of the model, where the system parameters and the disturbance parameter They are defined as:

[0020]

[0021]

[0022] in, is the dynamic disturbance, Respectively The hydrodynamic damping parameters of an underwater vehicle.

[0023] Furthermore, S2 is specifically:

[0024] Defining Followers and Navigator between t Relative distance between moments ,angle and relative azimuth as follows:

[0025]

[0026] In the formula, For the j An underwater vehicle in an earth-fixed coordinate system The centroid coordinates of

[0027] In order to ensure the connectivity and safety of underwater vehicles in the case of local communication, the following constraints are set:

[0028]

[0029] in, and It is the maximum sensing distance and angle determined by the limitations of the sensor; is the safety distance, which is predetermined by the size of the underwater vehicle and is the minimum distance to avoid collision;

[0030] In order to ensure constraint (6), the expected distance is introduced and the desired azimuth Define the distance error of the formation and angular error for:

[0031]

[0032] Among them, the expected distance and the desired azimuth It is the parameter expected by the designer, satisfying and ;

[0033] Substituting (6) into (7) we obtain:

[0034]

[0035] In control design, and The performance constraints are specified, that is, the formation tracking error is required to meet the following time-varying constraints:

[0036]

[0037] Among them, the boundary function and are the upper and lower limits of the distance error regulations, are the upper and lower limits specified for the angle error;

[0038] and ; The specific boundary function is expressed as:

[0039]

[0040] in, is the performance function, is the maximum steady-state error, is the designed positive parameter; ;

[0041] Differentiating equation (7) yields:

[0042]

[0043] in, is the yaw angle of the jth underwater vehicle, For the The difference between the yaw angle of the underwater vehicle and the relative angle of its pilot, is the relative angle difference between the yaw angle of the jth underwater vehicle and its follower. According to (6), we know and The value range is ;

[0044] Introducing the tan-type barrier Lyapunov function and :

[0045]

[0046] According to L'Hôpital's rule, taking the limit of equation (12) yields:

[0047]

[0048] Furthermore, the adaptive integral guidance strategy with LOS constraints under local communication is as follows:

[0049]

[0050] in, For the The expected forward velocity of the underwater vehicle, For the The expected yaw speed of the underwater vehicle, For the An estimate of the combined maximum forward and lateral velocities of an underwater vehicle, For the An estimate of the rate of change of the combined maximum forward and lateral velocities of an underwater vehicle. and are the design parameters, Intermediate parameters , intermediate parameters .

[0051] Furthermore, the finite time observer is as follows:

[0052]

[0053] in, Respectively The forward and yaw velocity errors of the underwater vehicle, and Respectively for and The estimated value of and Respectively The estimated rate of change of the forward and yaw velocity errors of the underwater vehicle, and Disturb parameters separately and The estimated value of and are disturbances, is a finite-time disturbance observer The estimated value of is a finite-time disturbance observer The estimated value of For The observed value of For Observed value of and are all observation errors, All are designed normal numbers;

[0054] Combine (13) (14) and The derivative is:

[0055]

[0056]

[0057] in, ; are all observation errors, , is a designed constant.

[0058] Furthermore, S5 is specifically:

[0059] Using non-singular global terminal sliding surface and Control torque and design.

[0060]

[0061] in, and they are all normal numbers, is a positive odd number and ;

[0062] By taking the derivative of equation (18), we can obtain:

[0063]

[0064] in, and They are the non-singular global terminal sliding surface and The rate of change, For the The rate of change of the desired forward velocity of the underwater vehicle, For the The rate of change of the desired yaw speed of the underwater vehicle.

[0065] In order to ensure the stability and convergence of the system, the control torque and Designed to:

[0066]

[0067]

[0068] in, are all constants.

[0069] A control system for an underwater vehicle formation based on LOS constraints comprises a processor, a memory and a computer program stored in the memory. When the processor executes the computer program, the steps in the underwater vehicle formation control method based on LOS constraints as described above are specifically performed.

[0070] The present invention has the following beneficial effects:

[0071] 1. The present invention introduces the obstacle Lyapunov function to process the line of sight (LOS) constraint, thereby ensuring the connectivity and safety of the underwater vehicle formation in a complex marine environment, thereby improving the accuracy and reliability of the formation control;

[0072] 2. The present invention can effectively suppress environmental interference and improve the tracking accuracy of the robot by designing an adaptive integral guidance rate and constructing a finite time observer, so that the system can show stronger robustness when facing external disturbances. Under the condition of LOS constraints in local communication, the collaborative operation of underwater vehicles can be realized, thereby improving the efficiency and effect of task execution;

[0073] 3. The present invention effectively prevents collisions between underwater vehicles and improves the safety of formation operations by setting strict line-of-sight constraints and safety distances. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] Figure 1 This is an example of a pilot-follower cluster control diagram in one embodiment of the present invention;

[0075] Figure 2 The figure is a flow chart of the method of the present invention. DETAILED DESCRIPTION

[0076] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments:

[0077] refer to Figure 1-2 In this embodiment, a method for controlling an underwater vehicle formation based on LOS constraints is provided, which is characterized by comprising the following steps:

[0078] S1: Establish a mathematical model for underwater vehicle formation control;

[0079] S2: Based on the mathematical model of underwater vehicles, the tracking error of the formation is defined, and the obstacle Lyapunov function is designed to handle the line-of-sight constraint to ensure the connectivity and safety of underwater vehicles;

[0080] S3: Design an adaptive integral guidance strategy with LOS constraints under local communication while satisfying connectivity and security.

[0081] S4: Construct a finite-time observer to fit the disturbance variables in the model and use the fitted approximation to compensate the control input of the system to suppress environmental disturbances and improve the tracking accuracy of the robot;

[0082] S5: Design a constrained formation controller based on a hybrid finite-time observer with an adaptive integral guidance strategy to achieve underwater vehicle formation control with LOS constraints under local communication.

[0083] In this embodiment, the underwater vehicle formation control mathematical model is a mathematical model of horizontal plane motion considering the three degrees of freedom of forward, lateral and yaw:

[0084] The underwater vehicle fleet includes underwater vehicles, numbered from 0 to ; Underwater vehicle As the leader, leading the overall fleet movement, the rest of the underwater vehicles are automatically controlled For followers, among them ;No. The dynamic equation of an underwater vehicle is as follows:

[0085]

[0086] in, For the An underwater vehicle in an earth-fixed coordinate system The centroid coordinates of and Respectively The underwater vehicle is in the Earth-fixed coordinate system and The velocity component in the direction, For the The yaw angle of the underwater vehicle, For the The rate of change of the yaw angle of an underwater vehicle over time, , ,and Respectively represent The forward, lateral and yaw velocities of the underwater vehicle are calculated by using the Lagrangian mechanics equations to add perturbations to the inertial parameters. and the hydrodynamic damping coefficient ;

[0087] Therefore, the dynamic model of the underwater vehicle is:

[0088]

[0089] In the formula, , and Respectively represent The forward, lateral and yaw velocity change rates of the underwater vehicle, and Respectively represent The forward moment and yaw moment of an underwater vehicle, is the inertia parameter of the model, where the system parameters and the disturbance parameter They are defined as:

[0090]

[0091]

[0092] in, is the dynamic disturbance, Respectively The hydrodynamic damping parameters of an underwater vehicle.

[0093] In this embodiment, S2 is specifically:

[0094] Defining Followers and Navigator between t Relative distance between moments ,angle and relative azimuth as follows:

[0095]

[0096] In the formula, For the j An underwater vehicle in an earth-fixed coordinate system The centroid coordinates of

[0097] In order to ensure the connectivity and safety of underwater vehicles in the case of local communication, the following constraints are set:

[0098]

[0099] in, and It is the maximum sensing distance and angle determined by the limitations of the sensor; is the safety distance, which is predetermined by the size of the underwater vehicle and is the minimum distance to avoid collision; it is worth noting that since the control rate proposed in this paper is non-singular, when In the case of , we need to avoid this situation by ensuring that the control constraint (6) is satisfied. Therefore, the parameter Must be satisfied.

[0100] In order to ensure constraint (6), the expected distance is introduced and the desired azimuth Define the distance error of the formation and angular error for:

[0101]

[0102] Among them, the expected distance and the desired azimuth It is the parameter expected by the designer, satisfying and ;

[0103] Substituting (6) into (7) we obtain:

[0104]

[0105] In control design, and The performance constraints are specified, that is, the formation tracking error is required to meet the following time-varying constraints:

[0106]

[0107] Among them, the boundary function and are the upper and lower limits of the distance error regulations, are the upper and lower limits specified for the angle error;

[0108] and ; The specific boundary function is expressed as:

[0109]

[0110] in, is the performance function, is the maximum steady-state error, is the designed positive parameter; ;

[0111] Differentiating equation (7) yields:

[0112]

[0113] in, is the yaw angle of the jth underwater vehicle, For the The difference between the yaw angle of the underwater vehicle and the relative angle of its pilot, is the relative angle difference between the yaw angle of the jth underwater vehicle and its follower. According to (6), we know and The value range is ;

[0114] Introducing the tan-type barrier Lyapunov function and :

[0115]

[0116] According to L'Hôpital's rule, taking the limit of equation (12) yields:

[0117]

[0118] In this embodiment, the adaptive integral guidance strategy with LOS constraints under local communication is specifically:

[0119]

[0120] in, For the The expected forward velocity of the underwater vehicle, For the The expected yaw speed of the underwater vehicle, For the An estimate of the combined maximum forward and lateral velocities of an underwater vehicle, For the An estimate of the rate of change of the combined maximum forward and lateral velocities of an underwater vehicle. and are the designed parameters, intermediate parameters , intermediate parameters .

[0121] In this embodiment, the finite time observer is as follows:

[0122]

[0123] in, Respectively The forward and yaw velocity errors of the underwater vehicle, and Respectively for and The estimated value of and Respectively An estimate of the rate of change of the forward and yaw velocity errors of the underwater vehicle, and Disturb parameters separately and The estimated value of and are disturbances, is a finite-time disturbance observer The estimated value of is a finite-time disturbance observer The estimated value of For The observed value of For Observed value of and are all observation errors, All of them are designed normal numbers;

[0124] Combine (13) (14) and The derivative is:

[0125]

[0126]

[0127] in, are all observation errors, is a designed constant.

[0128] In this embodiment, S5 is specifically:

[0129] Using non-singular global terminal sliding surface and Control torque and design.

[0130]

[0131] in, and they are all normal numbers, is a positive odd number and ;

[0132] By taking the derivative of equation (18), we can obtain:

[0133]

[0134] in, and They are the non-singular global terminal sliding surface and The rate of change, For the The rate of change of the desired forward velocity of the underwater vehicle, For the The rate of change of the desired yaw speed of the underwater vehicle.

[0135] In order to ensure the stability and convergence of the system, the control torque and Designed to:

[0136]

[0137]

[0138] in, are all constants.

[0139] It will be appreciated by those skilled in the art that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0140] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0141] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0142] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0143] The above is only a preferred embodiment of the present invention, and does not limit the present invention in other forms. Any technician familiar with the profession may use the above disclosed technical content to change or modify it into an equivalent embodiment with equivalent changes. However, any simple modification, equivalent change and modification made to the above embodiment according to the technical essence of the present invention without departing from the technical solution of the present invention still belongs to the protection scope of the technical solution of the present invention.

Claims

1. A method for controlling underwater vehicle formation based on LOS constraints, characterized in that: The following steps are involved: S1: Establish a mathematical model for underwater vehicle formation control; S2: Based on the mathematical model of underwater vehicles, the tracking error of the formation is defined, and the obstacle Lyapunov function is designed to handle the line-of-sight constraint to ensure the connectivity and safety of underwater vehicles; S3: Design an adaptive integral guidance strategy with LOS constraints under local communication while satisfying connectivity and security. S4: Construct a finite-time observer to fit the disturbance variables in the model and use the fitted approximation to compensate the control input of the system to suppress environmental disturbances and improve the tracking accuracy of the robot; S5: Design a constrained formation controller based on a hybrid finite-time observer with an adaptive integral guidance strategy to achieve formation control of underwater vehicles with LOS constraints under local communication; The mathematical model of underwater vehicle formation control is a mathematical model of horizontal plane motion considering three degrees of freedom: surge, drift, and bow pitch: Assume that the underwater vehicle formation consists of underwater vehicles, numbered from 0 to ; Underwater vehicle As the leader, leading the overall fleet movement, the rest of the underwater vehicles are automatically controlled For followers, among them ;No. The dynamic equation of an underwater vehicle is as follows: ； in, For the An underwater vehicle in an earth-fixed coordinate system The centroid coordinates of and Respectively The underwater vehicle is in the Earth-fixed coordinate system and The velocity component in the direction, For the The yaw angle of the underwater vehicle, For the The rate of change of the yaw angle of an underwater vehicle over time, ,and Respectively represent The forward, lateral and yaw velocities of the underwater vehicle are calculated by using the Lagrangian mechanics equations to add perturbations to the inertial parameters. and the hydrodynamic damping coefficient ; Therefore, the dynamic model of the underwater vehicle is: ； In the formula, and Respectively represent The forward, lateral and yaw velocity change rates of the underwater vehicle, and Respectively represent The surge moment and yaw moment of an underwater vehicle, is the inertia parameter of the model, where the system parameters and the disturbance parameter They are defined as: ； ； in, is the dynamic disturbance, Respectively The hydrodynamic damping parameters of an underwater vehicle; S2 is specifically: Defining Followers and Navigator between t Relative distance between moments ,angle and relative azimuth as follows: ； In the formula, For the j An underwater vehicle in an earth-fixed coordinate system The centroid coordinates of In order to ensure the connectivity and safety of underwater vehicles in the case of local communication, the following constraints are set: ； in, and It is the maximum sensing distance and angle determined by the limitations of the sensor; , is the safety distance, which is predetermined by the size of the underwater vehicle and is the minimum distance to avoid collision; In order to ensure constraint (6), the expected distance is introduced and the desired azimuth Define the distance error of the formation and angular error for: ； Among them, the expected distance and the desired azimuth is the parameter expected by the designer, satisfying and ; Substituting (6) into (7) we obtain: ； In control design, and The performance constraints are specified, that is, the formation tracking error is required to meet the following time-varying constraints: ； Among them, the boundary function and are the upper and lower limits of the distance error regulations, and are the upper and lower limits of the angle error regulations; and ; The specific boundary function is expressed as: ； in, is the performance function, is the maximum steady-state error, is the designed positive parameter; ; Differentiating equation (7) yields: ； in, is the yaw angle of the jth underwater vehicle, For the The difference between the yaw angle of the underwater vehicle and the relative angle of its pilot, is the relative angle difference between the yaw angle of the jth underwater vehicle and its follower; according to (6), we know and The value range is ; Introducing the tan-type barrier Lyapunov function and : ； According to L'Hôpital's rule, taking the limit of equation (12) yields: ; The adaptive integral guidance strategy with LOS constraints under local communication is specifically: ； in, For the The expected forward speed of the underwater vehicle, For the The expected yaw speed of the underwater vehicle, For the An estimate of the combined maximum forward and lateral speeds of an underwater vehicle, For the an estimate of the rate of change of the combined maximum forward and lateral velocities of the underwater vehicle; and are the designed parameters, intermediate parameters , intermediate parameters ; The finite-time observer is as follows: ； in, Respectively The forward and yaw velocity errors of the underwater vehicle, and Respectively for and The estimated value of and Respectively An estimate of the rate of change of the forward and yaw velocity errors of the underwater vehicle, and Disturb parameters separately and The estimated value of and are disturbances, is a finite-time disturbance observer The estimated value of is a finite-time disturbance observer The estimated value of For The observed value of For Observed value of and are all observation errors, All of them are designed normal numbers; Combine (13) (14) and The derivative is: ； in, are all observation errors, is a designed constant.

2. The underwater vehicle formation control method based on LOS constraint according to claim 1, characterized in that: The S5 is specifically: Using non-singular global terminal sliding surface and Control torque and design: ； in, and they are all normal numbers, is a positive odd number and ; By taking the derivative of equation (18), we can obtain: ； in, and They are the non-singular global terminal sliding surface and The rate of change, For the The rate of change of the desired forward speed of the underwater vehicle, For the The rate of change of the desired yaw velocity of the underwater vehicle; In order to ensure the stability and convergence of the system, the control torque and Designed to: ； in, are all constants.

3. An underwater vehicle formation control system based on LOS constraints, characterized in that: The method comprises a processor, a memory and a computer program stored in the memory. When the processor executes the computer program, the method specifically executes the steps in the underwater vehicle formation control method mainly based on LOS constraints as described in any one of claims 1 to 2.

Images