Multi-unmanned vehicle cooperative path tracking control method

Abstract

The application discloses a multi-unmanned vehicle cooperative path tracking control method, comprising the following steps: obtaining a desired path of a leader unmanned vehicle, and constructing a tangent coordinate system based on the desired path of the leader unmanned vehicle; based on the tangent coordinate system of the desired path of the leader unmanned vehicle, calculating tangent tracking errors and cross tracking errors of a follower unmanned vehicle according to real-time position information of the follower unmanned vehicle; obtaining desired values of the tangent tracking errors and the cross tracking errors according to a desired formation shape; obtaining a desired heading angle and a desired combined speed, so that the tangent tracking errors and the cross tracking errors of the follower unmanned vehicle converge to the corresponding desired values; and taking the desired heading angle and the desired combined speed as a tracking target, and controlling the follower unmanned vehicle through a thrust controller and a rudder angle controller based on a tracking differentiator. The application can improve the multi-unmanned vehicle cooperative path tracking precision, and make the follower unmanned vehicle converge to the desired relative position of the leader unmanned vehicle to maintain the desired formation shape.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned ship automatic control, and particularly relates to a multi-unmanned ship cooperative path tracking control method. BACKGROUND

[0002] As a new type of intelligent equipment, unmanned ships have the advantages of small size, high speed, small radar reflection area, etc., and can be deployed flexibly on rivers, lakes and seas. They are designed to perform various tasks such as underwater obstacle removal, patrol, equipment and material transportation, and emergency rescue. The unmanned design of unmanned ships enables them to perform tasks in complex hydrological and meteorological conditions, high-risk environments such as biochemical radiation, and therefore the use of unmanned ships to perform tasks can be safer and more efficient.

[0003] Considering that a single unmanned ship is difficult to perform complex tasks such as collective patrol, rescue, capture, monitoring and detection, multiple unmanned ships have the advantages of high efficiency, good flexibility, strong adaptability to complex environments, large coverage area and multiple types of tasks. In actual tasks, an expected path is often planned, and the unmanned ship cluster needs to track the expected path to reach the target location. Considering the requirements of the task and the interference of the complex environment, the unmanned ship cluster needs to form and switch the formation to improve the completion rate of the task.

[0004] In the prior art, if the dynamics of the unmanned ship is not considered, an observer or estimator based method is used to estimate the path variable and its change speed of the leader, for example, [Zhouhua Peng, Jun Wang and Dan Wang. Distributed Maneuvering of Autonomous Surface Vehicles Based on Neurodynamic Optimization and Fuzzy Approximation. 2018. 26(03):1083-1090], but the unmanned ship has under-actuated dynamics, making it difficult to apply this method in practice; considering the under-actuated characteristics, the path tracking task of multiple unmanned ships generally uses the traditional line-of-sight guidance method to track the path of a single unmanned ship, for example, [Nan Gu, Zhouhua Peng, Dan Wan, Yang Shi and Tianlin Wang. Antidisturbance Coordinated Path Following Control of Robotic Autonomous Surface Vehicles: Theory and Experiment [J]. IEEE / ASME Transactions on Mechatronics, 2022, 40(02): 2386-2396], which uses the traditional line-of-sight guidance law to design the desired heading angle, and cannot design the running speed, so this method can only satisfy the path tracking of the unmanned ship with a specific initial position, and the unmanned ships cannot maintain a specific formation. SUMMARY

[0005] The purpose of the present application is to provide a multi-unmanned ship cooperative path tracking control method, which realizes the cooperative path tracking control of multiple unmanned ships, improves the cooperative path tracking accuracy of multiple unmanned ships, and converges the follower unmanned ship to the expected relative position of the leader to maintain the expected formation. The technical solution adopted by the present application is as follows.

[0006] In one aspect, the present application provides a multi-unmanned ship cooperative path tracking control method, comprising:

[0007] Obtaining the expected path information of the leader unmanned ship, and constructing a tangent coordinate system based on the expected path of the leader;

[0008] Obtaining the real-time position information of the follower unmanned ship, and calculating the tangent tracking error and the cross tracking error of the follower unmanned ship based on the tangent coordinate system of the expected path of the leader and the real-time position information of the follower unmanned ship;

[0009] According to the desired formation shape, the desired values of the tangential tracking error and the cross tracking error of the follower unmanned vehicle in the tangent coordinate system are obtained;

[0010] The desired heading angle capable of making the cross tracking error of the follower unmanned vehicle converge to the desired value of the cross tracking error, and the desired resultant velocity capable of making the tangential tracking error converge to the desired value of the tangential tracking error are calculated;

[0011] The real-time heading angle and the real-time resultant velocity of the follower unmanned vehicle are obtained, and the follower unmanned vehicle is controlled by taking the desired heading angle and the desired resultant velocity as the tracking target, through the thrust controller and the rudder angle controller based on the tracking differentiator.

[0012] Optionally, the desired path of the leader unmanned vehicle is represented as a set of path points , wherein, is a path variable; and is a path point coordinate component;

[0013] The tangent coordinate system based on the desired path of the leader unmanned vehicle comprises: establishing the tangent coordinate system with the path point of the leader unmanned vehicle as the origin, the tangent direction of the path movement as the T axis, and the normal direction as the N axis;

[0014] The angle between the tangent of the leader path point and the north direction is calculated :

[0015]

[0016] wherein, is the first-order partial derivative of with respect to , and is obtained by calculating the derivative of and with respect to time, and the formula is:

[0017] ;

[0018] wherein, is the change speed of the path variable, i.e. is a function related to time, and satisfies . By setting , the forward speed of the leader can be controlled, and then the forward speed of the entire formation can be controlled.

[0019] Optionally, the tangential tracking error and the cross tracking error of the follower unmanned vehicle are calculated, and the formula is:

[0020] ;

[0021] The first The tangential tracking error of the follower UAV in the tangential coordinate system, The first The cross tracking error of the follower UAV in the tangential coordinate system, the first The real-time position of the follower UAV is represented as The angle between the tangent of the leader path point and the north direction.

[0022] Optionally, the desired heading angle is obtained by using a line-of-sight guidance method, and is represented as:

[0023] Wherein:

[0024]

[0025]

[0026] In the formula, is the desired heading angle, is the angle between the tangent of the leader path point and the north direction, is the side slip angle of the first UAV, are the forward speed and the lateral speed of the first UAV in the body coordinate system, respectively, are the desired tangential tracking error and the desired cross tracking error of the follower UAV in the tangential coordinate system, respectively, is the line-of-sight term, is a constant, , is a parameter to be tuned, and satisfies .

[0027] Optionally, the desired resultant speed is represented as:

[0028]

[0029] In the formula, is the desired resultant speed, is the change speed of the path variable, and the intermediate variable ,

[0030] Optionally, the follower UAV is controlled by using a tracking differentiator-based thrust controller and a rudder angle controller, with the desired heading angle and the desired resultant speed as tracking targets, and the control includes:

[0031] The desired heading angular velocity is calculated according to the desired heading angle;

[0032] The differential of the desired resultant speed and the differential of the desired heading angular velocity are estimated by using a tracking differentiator, respectively;​

[0033] based on the dynamics model of the USV, the tracking error of the resultant velocity and the estimated value of the derivative of the desired resultant velocity to design the thrust controller;

[0034] based on the dynamics model of the USV, the tracking error of the heading angle and the estimated value of the derivative of the desired heading angle velocity to design the rudder angle controller.

[0035] Optionally, the derivative of the desired resultant velocity and the derivative of the desired heading angle velocity are estimated by using a tracking differentiator respectively, wherein the tracking differentiator for estimating the derivative of the desired resultant velocity is represented as:

[0036]

[0037] wherein, is the estimated value of the desired resultant velocity , is the estimated value of , is the derivative of with respect to time, is an adjustable estimator parameter;

[0038] the tracking differentiator for estimating the derivative of the desired heading angle velocity is represented as:

[0039]

[0040] wherein, is the estimated value of the desired heading angle velocity , is the estimated value of , is the derivative of with respect to time, is an adjustable estimator parameter

[0041] Optionally, the dynamics model of the USV is represented as:

[0042]

[0043] wherein, is the derivative of the forward velocity , the lateral velocity and the angular velocity of the USV with respect to time, and are the thrust controller and the rudder angle controller of the follower USV respectively, is the dynamics model parameter of the USV, which can be obtained by identification.

[0044] The thrust controller is represented as follows:

[0045]

[0046] In the formula, For controller gain parameters; the first The expected combined velocity tracking error of an unmanned surface vessel , For the first The combined speed of the unmanned surface vessel For the first The expected combined speed of an unmanned surface vessel; yes The estimated value, yes The derivative with respect to time. Under the designed thrust control, the resultant velocity of the unmanned surface vessel will accurately track the designed desired resultant velocity.

[0047] Optionally, the rudder angle controller is represented as:

[0048]

[0049] In the formula, The controller parameters to be tuned; Bow angle tracking error of an unmanned surface vessel , For the unmanned surface vessel, the heading angle is denoted by angular velocity; the heading angular velocity tracking error is also denoted by angular velocity. , It is the derivative of the desired heading angular velocity. yes The estimated value, yes The derivative with respect to time. Under the designed rudder angle controller, the heading angle of the follower unmanned surface vessel can accurately track the designed desired heading angle.

[0050] In a second aspect, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the multi-unmanned surface vessel cooperative path tracking control method as described in the first aspect.

[0051] Beneficial effects

[0052] Compared with existing technologies, the advantages of this invention are:

[0053] 1) This invention designs a novel line-of-sight guidance law, and thereby designs the desired heading angle and desired resultant velocity, which can ensure that the follower unmanned surface vessel can converge to the desired position from any initial position, so that all unmanned surface vessels can perform cooperative path tracking tasks in any initial state and can change any formation.

[0054] 2) The present application establishes a tangent coordinate system based on the leader path, designs the tracking error expectation value of the follower according to the expected formation, and combines the designed expected bow angle and expected combined speed to ensure that all unmanned ships keep the formation neat at all times while tracking any path. This not only helps to improve the task completion rate, but also enhances the robustness and reliability of the unmanned ship cluster in the face of complex ocean environments.

[0055] 3) The unmanned ship thrust controller and rudder angle controller are designed based on the tracking differentiator, which can simplify the design of the controller, reduce the complexity of the calculation, and be friendly to the hardware environment. BRIEF DESCRIPTION OF DRAWINGS

[0056] Figure 1 Fig. 1 is a flowchart of the multi-unmanned ship cooperative path tracking control method of the present application;

[0057] Figure 2 Fig. 2 is a specific implementation flowchart of the multi-unmanned ship cooperative path tracking control method in an embodiment of the present application;

[0058] Figure 3 Fig. 3 is a schematic diagram of the principle of the tangent tracking error and the cross tracking error of the unmanned ship and their expected values in the tangent coordinate system;

[0059] Figure 4 Fig. 4 is a path diagram of three unmanned ships performing cooperative path tracking according to the set formation.

[0060] Figure 5 Fig. 5 is the tangent / cross tracking error of the three unmanned ships.

[0061] Figure 6 Fig. 6 is the expected bow angle and the actual bow angle of the three unmanned ships.

[0062] Figure 7 Fig. 7 is the expected combined speed and the actual combined speed of the three unmanned ships. DETAILED DESCRIPTION

[0063] The following is further described in combination with the drawings and specific embodiments.

[0064] The technical concept of the present application is that, on the one hand, through the expected bow angle and the expected combined speed designed based on the new line-of-sight guidance law, the follower unmanned ship can converge to the expected position from any initial position, so that all unmanned ships can perform cooperative path tracking tasks in any initial state and can perform any formation transformation. On the other hand, by establishing a tangent coordinate system based on the leader path, the expected value of the tracking error of the follower is determined according to the expected formation, and the designed expected bow angle and expected combined speed are combined to ensure that all unmanned ships keep the formation neat at all times while tracking any path.

[0065] Embodiment 1

[0066] Reference Figure 1 The multi-unmanned ship cooperative path tracking control method of the embodiment comprises:

[0067] S1, obtaining the expected path information of the leader unmanned ship, describing the expected trajectory of the leader, and constructing the tangent coordinate system based on the expected path of the leader.

[0068] S2, obtaining the real-time position information of the follower unmanned ship, calculating the tangential tracking error and the cross tracking error of the follower unmanned ship based on the tangent coordinate system of the leader expected path according to the real-time position information of the follower unmanned ship.

[0069] S3, obtaining the expected values of the tangential tracking error and the cross tracking error of the follower unmanned ship in the tangent coordinate system according to the expected formation.

[0070] S4, calculating the expected heading angle that can make the cross tracking error of the follower unmanned ship converge to the expected value of the cross tracking error, and the expected total speed that can make the tangential tracking error converge to the expected value of the tangential tracking error.

[0071] S5, obtaining the real-time heading angle and real-time total speed of the follower unmanned ship, taking the expected heading angle and the expected total speed as the tracking target, and controlling the follower unmanned ship through the thrust controller and the rudder angle controller based on the tracking differentiator.

[0072] In the embodiment, the leader unmanned ship can be a virtual leader, which is only set for constructing the expected path and the basic tangent coordinate system.

[0073] The technical solution of the embodiment can enable the multi-unmanned ship to achieve accurate path tracking while maintaining the expected formation.

[0074] Embodiment 2

[0075] Based on Embodiment 1, the embodiment specifically introduces the implementation process of the multi-unmanned ship cooperative path tracking control method, and reference Figure 2 , which mainly includes the following contents.

[0076] S1, describing the expected path of the leader and constructing the tangent coordinate system based on the path of the leader.

[0077] S11, defining the leader path function

[0078] Define the movement of the leader on a parameterized path Let be the path variable, then each path variable The coordinates of a point on the corresponding path are , and are functions related to .

[0079] S12, construct the tangent coordinate system and calculate the tangent angle between the leader path point tangent and the north

[0080] Calculate and The derivative with respect to time is:

[0081] ;

[0082] Where: is the change speed of the path variable, which is also a function related to time , and needs to meet the condition When setting , the leader's forward speed can be controlled, and then the entire formation's forward speed can be controlled; are the first-order partial derivatives of with respect to the path variable , then the tangent angle between the moving tangent direction of the path point at this moment and the north can be calculated as:

[0083] .

[0084] S2, based on the tangent coordinate system of the leader path, calculate the tangent / cross tracking error of the follower unmanned surface vehicle.

[0085] The relative position between the leader and the follower unmanned surface vehicle is described in the tangent coordinate system, and the first follower unmanned surface vehicle can obtain its own position information according to the combined navigation positioning sensor, and convert the position tracking error in the world coordinate system to the tangent coordinate system, so the tangent / cross tracking error of the first follower unmanned surface vehicle can be calculated:

[0086] ;

[0087] Where: is the tangent tracking error of the first unmanned surface vehicle in the tangent coordinate system, is the cross tracking error of the first unmanned surface vehicle in the tangent coordinate system.

[0088] S3, set the expected value of the tangent tracking error and the cross tracking error according to the expected formation shape

[0089] ReferenceFigure 3 As shown in the figure, a tangent coordinate system is established with the leader as the origin, the tangent direction of the path as the x-axis and the normal direction of the path as the y-axis. Then the tangent tracking error and the cross tracking error of the follower unmanned vehicle in the tangent coordinate system are defined as The first unmanned vehicle is described in the first aspect of the application The current coordinates of the unmanned vehicle in the tangent coordinate system are described in the second aspect of the application. According to the desired formation, the desired position of the unmanned vehicle relative to the leader can be obtained, and further the desired coordinates of the unmanned vehicle in the tangent coordinate system are obtained That is, according to the desired formation, the desired values of the tangent tracking error and the cross tracking error of the follower unmanned vehicle in the tangent coordinate system are set .

[0090] If the following conditions are met That is, when the following conditions are met, the multiple unmanned vehicles can achieve cooperative path tracking while maintaining the desired formation:

[0091] .

[0092] Wherein: , is a very small positive number.

[0093] By changing the desired values of the tangent tracking error and the cross tracking error The formation of the multiple unmanned vehicles can be changed.

[0094] S4, the desired heading angle and the desired combined speed are designed so that the tangent tracking error and the cross tracking error of the follower unmanned vehicle converge to the desired values set in S2.

[0095] S41, based on the kinematic equation of the unmanned vehicle and the tangent tracking error and the cross tracking error of the follower unmanned vehicle calculated in S2, the rates of change of the tangent tracking error and the cross tracking error are calculated.

[0096] Consider the following kinematic equation of the unmanned vehicle:

[0097] ;

[0098] Wherein: is the derivative with respect to time, is the heading angle of the unmanned vehicle, are the forward speed, lateral speed and angular speed of the unmanned vehicle body coordinate system respectively. Based on the kinematic equation of the unmanned vehicle, the tangent tracking error and the cross tracking error are differentiated to obtain:

[0099] Based on the kinematic equation of the unmanned vehicle, the tangent tracking error and the cross tracking error are differentiated to obtain:

[0100] ;

[0101] Wherein: is​​ derivative of the path tangent with respect to time, the desired heading angle of the the desired velocity of the the desired side-slip angle of the , the derivative of the path tangent with respect to time, the derivative of the path tangent with respect to time, the derivative of the path tangent with respect to time,

[0102] ;

[0103] where: the second-order partial derivative of the path coordinate with respect to the path variable .

[0104] By tracking the error rate, a reasonable guidance law and velocity design can be achieved, so that the tangential and cross tracking errors converge to the desired value.

[0105] S42, separate the tracking error of the desired velocity and the desired heading angle of the first unmanned ship in step S22

[0106] Considering the tracking error of the desired velocity and the desired heading angle, the differential of the tangential tracking error and the cross tracking error can be further converted as:

[0107] ;

[0108] where: the derivative of the path tangent with respect to time, and the desired heading angle and the desired velocity of the , the desired heading angle tracking error and the desired velocity tracking error of the , the desired heading angle tracking error and the desired velocity tracking error of the , and are functions related to , and the specific forms are as follows:

[0109] ;

[0110] From the above formula, when the heading angle tracking error and the velocity tracking error converge to 0, , . The control target is to design the desired heading angle and the desired velocity of the unmanned ship, so that the tangential / cross tracking error converges to the desired value .

[0111] S43, design the desired heading angle so that the follower USV cross tracking error converges to the desired value

[0112] Based on the new line-of-sight guidance method, the first desired heading angle of the follower USV is:

[0113] ;

[0114] wherein: is the designed line-of-sight term, is a normal number, , is an adjustable parameter and satisfies the value condition , is the hyperbolic tangent function, and the specific expression form is as follows:

[0115] ;

[0116] Substitute the desired heading angle into the in S42, when the desired combined velocity , by adjusting the parameters and values so that the cross tracking error converges to .

[0117] The parameter setting process is as follows: first, according to the desired formation information and the curvature at the path variable , the disturbance variable is calculated, and the specific expression forms of the disturbance variable and the curvature are as follows:

[0118] ,

[0119] .

[0120] Secondly, under the above value condition, select appropriate parameter and values to satisfy the following conditions:

[0121] ,

[0122] The closer to 1, the closer to 2, the greater the value, so there must be a set of appropriate parameters that can satisfy the above conditions.

[0123] S44, design the desired surge velocity such that the tangential tracking error of the follower USV converges to the desired value ;

[0124] Substitute the desired heading angle into the S31 equation, design the desired surge velocity signal such that converges to the desired value in both cases of greater than 0 and less than 0:

[0125] ;

[0126] The designed desired surge velocity is a piecewise function, and is continuous at Substitute the desired surge velocity into the S31 equation can guarantee that the tangential tracking error of the first follower USV converges to the desired value .

[0127] S5, design the thrust controller and rudder angle controller based on the tracking differentiator to track the desired surge velocity and desired heading angle designed in S4.

[0128] S51, calculate the desired heading angle velocity using the desired heading angle in step S4

[0129] Differentiate the desired heading angle designed in S43:

[0130] ;

[0131] where: is the derivative of with respect to time, is the derivative of with respect to time, is the derivative of with respect to time, and the specific form is as follows:

[0132] .

[0133] S52, design the tracking differentiator to estimate the derivative of the desired heading angle velocity and the derivative of the desired surge velocity

[0134] Design the tracking differentiator of the following equation to estimate the derivative of the desired surge velocity:

[0135] ;

[0136] where: is the estimated value of , is the estimated value of , is the estimator parameter to be tuned.​​​​

[0137] Similarly, a tracking differentiator designed as follows can estimate the derivative of the desired heading angular velocity:

[0138] ;

[0139] in: yes The estimated value, yes The estimated value, These are the estimator parameters to be tuned.

[0140] Compared to directly calculating the differential or using the traditional finite difference method, the designed tracking differentiator can accurately estimate the differential of the desired resultant velocity. Differential of expected heading angular velocity This reduces computational complexity without amplifying sensor noise.

[0141] S53, designed a thrust controller to ensure that the unmanned surface vessel's resultant velocity keeps pace with the desired resultant velocity.

[0142] Consider the following: Dynamics model of an unmanned surface vessel:

[0143] ;

[0144] in: for The derivative with respect to time, and These are the thrust controller and rudder angle controller for the follower unmanned surface vessel. The model parameters for the unmanned surface vessel are obtained through identification.

[0145] Based on dynamic model and resultant velocity tracking error and S41 The estimated thrust controller is designed as follows:

[0146] ;

[0147] in: This refers to the controller gain parameter. Under the designed thrust control, the resultant velocity of the unmanned surface vessel will accurately track the desired resultant velocity designed in S33.

[0148] S54, designed with a rudder angle controller to ensure the unmanned surface vessel's heading angle accurately tracks the desired heading angle.

[0149] Define the heading angular velocity tracking error as: .

[0150] Based on the dynamic model in S41, heading angle tracking error and S41 in The rudder angle controller designed in S42 is as follows:

[0151] ;

[0152] wherein: is the controller parameter to be tuned. Under the designed rudder angle controller, the heading angle of the follower USV can accurately track the desired heading angle designed in S32.

[0153] Effect verification

[0154] This embodiment is applied to a test scenario, MATLAB 2022a is used as a simulation calculation software to simulate the motion of the USV in three-dimensional space, and the USV is equipped with an inertial navigation sensor including an accelerometer, a magnetometer and a gyroscope. In the simulation environment, the desired “U” type path is set as: , then the path function of the virtual leader is as follows:

[0155]

[0156] wherein: the initial value of the path variable is set to -11, and the path variable change rate is set to 0.8 m / s.

[0157] The simulation adopts three USVs for cooperative path tracking. In order to verify the effectiveness of the designed algorithm, the initial positions of the three USVs are set at three different positions of the virtual leader: the initial position of USV1 is: ; the initial position of USV2 is: ; and the initial position of USV3 is: . During the path tracking process, the three USVs need to form a triangular formation at the beginning, and after 12s, the formation is switched from a triangular formation to a straight line formation, and then at 24s, the formation is switched from a straight line formation to a triangular formation. The desired tangential / crossing tracking error according to the formation is set as follows: for the triangular formation: ;

[0158] for the straight line: . In order to prevent the influence caused by the sudden change of the desired tangential / crossing tracking error, the desired value has a switching time of 1s during each formation switching.

[0159] In the simulation environment, the parameters in the line-of-sight guidance law are as follows: , , ; the gain of the tracking differentiator is set as: , ; the controller parameters in the thrust controller and the rudder angle controller are set as: .

[0160] Figure 3 The cooperative path tracking diagram of the three unmanned surface vehicles is given, the thick black line and the white line frame in the diagram are the pool map, the black solid line in the diagram is the moving path of the virtual leader, the black dot-dash line is the moving path of USV1, the black dot line is the moving path of USV2, and the black dashed line is the moving path of USV3, and it can be seen from the diagram that the cooperative path tracking control method proposed in the application can ensure that the three unmanned surface vehicles can form the designed formation for path tracking from any initial position, and can also change the formation.

[0161] Figure 4 The tangential / cross tracking errors of the three unmanned surface vehicles are given, the black solid line is the tangential / cross tracking error of USV1, the dashed line is the tangential / cross tracking error of USV2, and the dot line is the tangential / cross tracking error of USV3. Figure 4 It can be seen from the diagram that after the formation switching command is issued at 12s and 24s, the tangential / cross tracking errors of the USVs will change smoothly to the expected value, and there will be no overshoot or chattering instability, which proves the effectiveness of the method proposed in the application.

[0162] Figure 5 The expected heading angle of the three unmanned surface vehicles and the tracking situation thereof are given, the black solid line represents the expected heading angle of the unmanned surface vehicle given by the new type of line-of-sight guidance law in S32, and the black dashed line represents the actual heading angle of the unmanned surface vehicle, and it can be seen from the simulation result that the two lines are nearly coincident, which shows that the designed rudder angle controller can quickly and accurately track the expected heading angle.

[0163] Figure 6 The expected combined speed of the three unmanned surface vehicles and the tracking situation thereof are given, the black solid line represents the expected combined speed of the unmanned surface vehicle designed in S33, and the black dashed line represents the actual combined speed of the unmanned surface vehicle, and it can be seen from the simulation result that the two lines are nearly coincident, which shows that the designed thrust controller can quickly and accurately track the expected combined speed.

[0164] It can be obviously seen from the simulation result that the method of the water surface multi-unmanned surface vehicle cooperative path tracking based on the new type of line-of-sight guidance method proposed in the application can quickly and accurately track the expected path, and can form the expected formation and change the formation.

[0165] Embodiment 3

[0166] This embodiment introduces a computer readable storage medium, which stores a computer program, and the computer program is executed by a processor to realize the multi-unmanned surface vehicle cooperative path tracking control method as introduced in Embodiment 1 or Embodiment 2.

[0167] Those skilled in the art will appreciate that embodiments of the application can be readily used as software, hardware, or a combination of software and hardware. In one

[0168] The computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart block or blocks. Figure 1 one or more flowcharts and / or blocks in the flowcharts and / or a combination of flowcharts and / or blocks in the flowcharts can be implemented by computer program instructions. Figure 1 an apparatus with a means for performing each function specified in the flowchart block or blocks.

[0169] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instructions which implement the function specified in the flowchart block or blocks. Figure 1 one or more flowcharts and / or blocks in the flowcharts and / or a combination of flowcharts and / or blocks in the flowcharts can be implemented by computer program instructions. Figure 1 an apparatus with a means for performing each function specified in the flowchart block or blocks.

[0170] The computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart block or blocks. Figure 1 one or more flowcharts and / or blocks in the flowcharts and / or a combination of flowcharts and / or blocks in the flowcharts can be implemented by computer program instructions. Figure 1 an apparatus with a means for performing each function specified in the flowchart block or blocks.

[0171] The embodiments of the application described above are intended to be merely exemplary and those skilled in the art will recognize various modifications which can be made to the application without departing from the scope of the application intended to be limited only by the broadest interpretation of the appended claims.

Claims

1. A multi-unmanned surface vessel cooperative path tracking control method, characterized in that, include: Obtain the desired path information of the leader unmanned surface vessel and construct a tangent coordinate system based on the leader's desired path; Obtain the real-time position information of the follower UAV, and calculate the tangential tracking error and cross tracking error of the follower UAV based on the tangential coordinate system of the leader's expected path and the real-time position information of the follower UAV. Based on the desired formation, obtain the expected values ​​of the tangential tracking error and cross tracking error of the follower unmanned surface vessel in the tangential coordinate system; Calculate the desired heading angle that allows the cross-tracking error of the follower unmanned surface vessel to converge to the desired value of the cross-tracking error, and the desired resultant velocity that allows the tangential tracking error to converge to the desired value of the tangential tracking error; The real-time heading angle and real-time resultant velocity of the follower unmanned surface vessel are obtained. The desired heading angle and desired resultant velocity are used as the tracking target. The follower unmanned surface vessel is controlled by a thrust controller and a rudder angle controller based on the tracking differentiator. The desired path of the leader unmanned surface vessel is represented by path points. The set of, where, For path variables; and These are the coordinate components of the path points; The construction of the tangent coordinate system based on the leader's expected path includes: establishing a tangent coordinate system with the path point of the leader's unmanned surface vessel as the origin, the tangent direction of the path movement as the T-axis, and the normal direction as the N-axis. Calculate the angle between the tangent of the leader path point and the north direction. : ， in, They are right The first-order partial derivative is calculated by... and The derivative with respect to time is obtained by the formula: ； in, The rate of change of the path variable, i.e. It is a time-dependent function and satisfies ; The formulas for calculating the tangential tracking error and cross-tracking error of the follower unmanned surface vessel are as follows: ； For the first Tangential tracking error of a follower unmanned surface vessel in tangential coordinate system For the first The cross-tracking error of the follower unmanned surface vessel in the tangential coordinate system, the first The real-time position of the follower unmanned surface vessel is represented as ; The desired heading angle is obtained using the line-of-sight guidance method and is expressed as: ,in: ， ， In the formula, For the desired heading angle, The angle between the tangent line to the leader's path point and the north direction. For the first The sideslip angle of an unmanned surface vessel They are the first The forward and lateral velocities of the unmanned surface vessel in its hull coordinate system. These represent the expected values ​​of the tangential tracking error and the cross-tracking error of the follower unmanned surface vessel in the tangential coordinate system, respectively. For the line of sight, For positive integers, , The parameter to be tuned satisfies the following conditions: ; The desired resultant velocity is expressed as: ， In the formula, For the desired resultant speed, The rate of change of path variables, intermediate variables .

2. The method according to claim 1, characterized in that, The method of using the desired heading angle and desired resultant velocity as tracking targets, and controlling the follower unmanned surface vessel through a thrust controller and rudder angle controller based on a tracking differentiator, includes: Calculate the desired heading angular velocity based on the desired heading angle; The derivatives of the desired resultant velocity and the desired heading angular velocity are estimated using a tracking differentiator, respectively. Based on the dynamic model of unmanned surface vessels and the resultant velocity tracking error and the differential of the expected velocity The estimated value is used to design the thrust controller; Based on the dynamic model of the unmanned surface vessel and the heading angle tracking error Differential of expected heading angular velocity The estimated value is used to design the rudder angle controller.

3. The method according to claim 2, characterized in that, The method involves using a tracking differentiator to estimate the derivatives of the desired resultant velocity and the desired heading angular velocity, respectively. The tracking differentiator used to estimate the derivative of the desired resultant velocity is defined as follows: ， In the formula, It is the expected sum velocity The estimated value, yes The estimated value, These are tuneable estimator parameters; The tracking differentiator used to estimate the derivative of the desired prime minister's angular velocity is represented as: ， In the formula, It is the desired heading angular velocity. The estimated value, yes The estimated value, These are tuned estimator parameters.

4. The method according to claim 3, characterized in that, The dynamic model of the unmanned surface vessel is expressed as follows: ， In the formula, For the forward speed of the unmanned surface vessel Lateral velocity and angular velocity The derivative with respect to time, and These are the thrust controller and rudder angle controller for the follower unmanned surface vessel. These are the dynamic model parameters for the unmanned surface vessel; The thrust controller is represented as follows: ， In the formula, For controller gain parameters; the first The expected combined velocity tracking error of an unmanned surface vessel , For the first The combined speed of the unmanned surface vessel For the first The expected combined speed of an unmanned surface vessel; yes The estimated value, yes The derivative with respect to time.

5. The method according to claim 3, characterized in that, The dynamic model of the unmanned surface vessel is expressed as follows: ， In the formula, For the forward speed of the unmanned surface vessel Lateral velocity and angular velocity The derivative with respect to time, and These are the thrust controller and rudder angle controller for the follower unmanned surface vessel. These are the dynamic model parameters for the unmanned surface vessel; The rudder angle controller is represented as: ， In the formula, The controller parameters to be tuned; Bow angle tracking error of an unmanned surface vessel , For the unmanned surface vessel, the heading angle is denoted by angular velocity; the heading angular velocity tracking error is also denoted by angular velocity. , It is the derivative of the desired heading angular velocity. yes The estimated value, yes The derivative with respect to time.

6. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the multi-unmanned surface vessel cooperative path tracking control method as described in any one of claims 1-5.

Images