Unmanned ship encirclement and countermeasure method based on shepherding algorithm

Abstract

The present invention relates to the technical field of unmanned ship encirclement and confrontation, and discloses an unmanned ship encirclement and confrontation method based on a shepherding algorithm, comprising the following steps: (1) establishing a kinematic and dynamic model of an unmanned ship; (2) controlling the unmanned ship through the Shepherding algorithm, combining the Shepherding algorithm with an ADRC controller, obtaining the forward thrust and steering thrust of the unmanned ship, and realizing the movement of the unmanned ship; (3) dividing the unmanned ships into our unmanned ships and enemy unmanned ships, with our unmanned ships acting as sheepdogs and enemy unmanned ships acting as sheep, and realizing the unmanned ship encirclement and confrontation control through step (2). The present invention combines the Shepherding algorithm with an ADRC controller, obtains the forward thrust and steering thrust of the unmanned ships, realizes the movement of the unmanned ships, controls the unmanned ships to maintain a good distance, effectively avoids collisions, controls our unmanned ships to encircle enemy unmanned ships, and the enemy unmanned ships perform confrontational movements and are eventually driven to a target area.

Description

Technical Field

[0001] The present invention relates to a method for encircling and confronting an unmanned surface vehicle (USV), belonging to the technical field of encircling and confronting an unmanned surface vehicle. Background Art

[0002] In the prior art, there are several methods for encircling and capturing by unmanned boats based on the principles as follows.

[0003] 1. Collaborative roundup method based on path planning

[0004] The path planning-based collaborative capture method refers to multiple unmanned ships working together to capture target ships or objects according to a pre-set path planning algorithm. This method uses the collaborative operations between unmanned ships, allowing them to act together according to predetermined strategies and algorithms to achieve efficient target capture.

[0005] This method has the following shortcomings:

[0006] 1. Communication delay: If the communication channel is unstable or delayed, the timeliness of path planning and coordinated actions may decrease, affecting the efficiency of task execution.

[0007] 2. Difficulty in dealing with emergencies: Some emergencies may be difficult to deal with through pre-planned paths, requiring real-time adjustment of path planning, increasing the complexity of task execution.

[0008] 3. Path conflict: When multiple unmanned ships act in coordination, path conflicts may occur, which requires good coordination and planning to avoid collisions or confusion.

[0009] 4. Increased energy consumption: Multiple unmanned ships executing path planning at the same time may increase energy consumption, and it is necessary to consider how to optimize the algorithm to reduce energy consumption.

[0010] 2. Collaborative hunting method based on artificial neural network

[0011] The collaborative capture method based on artificial neural network refers to the use of neural network technology to achieve collaborative operations between multiple unmanned ships to perform capture tasks more intelligently and adaptively. This method uses the learning and decision-making capabilities of neural networks to enable unmanned ships to work together more intelligently and adapt to different environments and mission requirements.

[0012] This method has the following shortcomings:

[0013] 1. Data requirements: Neural networks require a lot of data for training, and the training process can be time-consuming.

[0014] 2. Explainability: Neural network decisions have poor explainability, and it is difficult to understand their specific decision-making process.

[0015] 3. Overfitting: Overfitting problem may occur, resulting in poor performance of the model in the actual environment.

[0016] 3. Collaborative hunting method based on swarm intelligence algorithm

[0017] The collaborative capture method based on swarm intelligence algorithm is a method that uses swarm intelligence algorithm to achieve collaborative operations between multiple unmanned ships to better perform capture tasks. This method simulates the algorithm of group behavior in nature, enabling unmanned ships to work together intelligently and adapt to complex mission environments and dynamic targets.

[0018] This method has the following shortcomings:

[0019] 1. Parameter tuning: There are a large number of parameters in the swarm intelligence algorithm that need to be tuned, and the tuning process may be cumbersome.

[0020] 2. Local optimum: The algorithm may fall into a local optimal solution, affecting the effectiveness of the team’s collaborative roundup.

[0021] 3. Computational complexity: Some swarm intelligence algorithms have high computational complexity and real-time issues need to be fully considered.

[0022] 4. Collaborative Encirclement Method Based on Artificial Potential Field

[0023] The collaborative capture method based on artificial potential field is a method that uses virtual potential field simulation technology to achieve collaborative capture of targets between multiple unmanned ships. In this method, each unmanned ship is regarded as a force point. Under the action of the virtual potential field, the unmanned ship will adjust its course according to the gradient direction of the potential field to achieve the capture of the target.

[0024] This method has the following shortcomings:

[0025] 1. Local optimum: It may fall into the local optimal solution, affecting the capture effect.

[0026] 2. Design complexity: The attractive field and repulsive field need to be carefully designed, and parameter adjustment may be cumbersome.

[0027] 3. Number of targets: Artificial potential field methods may be challenging in dealing with multiple targets.

[0028] 5. Collaborative hunting method based on genetic algorithm

[0029] The collaborative capture method based on genetic algorithm is a method that uses genetic algorithm to optimize the collaborative actions between multiple unmanned ships to achieve effective capture goals. In this method, genetic algorithm is used to optimize the action strategy and collaborative mode of unmanned ships to maximize the capture efficiency of the team.

[0030] This method has the following shortcomings:

[0031] 1. Parameter adjustment: It is difficult to select genetic algorithm parameters and it is difficult to achieve the optimal state.

[0032] 2. Computational overhead: In complex environments, genetic algorithms may require high computational overhead.

[0033] 3. Local optimum: Sometimes you will fall into the local optimal solution and it will be difficult to find the global optimal solution. Summary of the invention

[0034] In view of the shortcomings of the existing unmanned ship encirclement and confrontation technology, the present invention provides an unmanned ship encirclement and confrontation method based on the shepherding algorithm. During the movement of the unmanned ships, the method can control two unmanned ships to maintain a good distance and effectively avoid collision. At the same time, sufficient forward and turning thrust can be calculated for the movement of the unmanned ships to meet the needs of the encirclement and confrontation process.

[0035] The unmanned boat encirclement and countermeasure method based on shepherding algorithm of the present invention comprises the following steps:

[0036] (1) Establish the kinematic and dynamic models of the unmanned ship;

[0037] (2) The unmanned ship is controlled by the Shepherding algorithm. The Shepherding algorithm is combined with the ADRC controller (auto-disturbance rejection controller) to obtain the forward thrust and steering thrust of the unmanned ship, thereby realizing the movement of the unmanned ship.

[0038] (3) The unmanned ships are divided into friendly unmanned ships (USVs) and enemy unmanned ships (USVs). The friendly unmanned ships are used as sheepdogs and the enemy unmanned ships are used as sheep. The unmanned ship encirclement and confrontation control is achieved through step (2).

[0039] The specific process of step (1) of establishing the kinematic and dynamic models of the unmanned ship is:

[0040] When the unmanned ship moves on the sea, it moves with six degrees of freedom and establishes a geodetic coordinate system fixed on the ground. and the attached coordinate system fixed to the hull ; Geodetic coordinate system Used to describe the location information of the unmanned ship, the origin of the coordinate system Anywhere on the sea surface, The axis points to the true north of the earth, which is defined as the positive direction of the unmanned ship's movement. The axis points to the east of the earth, which is to the right of the direction of movement of the unmanned ship. Axis perpendicular to sea level The plane points downward, which is the direction in which the unmanned ship moves up and down; the attached coordinate system Used to describe the attitude information of the unmanned ship. The origin of the coordinate system is located at the center of gravity of the unmanned ship. The axis is located on the centerline of the hull and points to the front of the hull. The axis is perpendicular to the centerline of the hull and points to the right side of the hull. Perpendicular to The plane points to the underside of the hull;

[0041] The location of unmanned ships in the ocean Coordinates are used to represent the posture during movement, and the heel angle is used to represent the posture during movement. , pitch angle , Bow Angle To express, that is Indicates the attitude information of the unmanned ship, The speed of the direction of movement is Indicates, around The angular velocity of the direction of rotation is Indicates that towards The speed of the direction of movement is Indicates, around The angular velocity of the direction of rotation is Indicates that towards The speed of the direction of movement is Indicates, around The angular velocity of the direction of rotation is express;

[0042] Only considering the movement posture of the unmanned ship on the same plane (i.e., not considering the up and down movement, rotation posture around the x-axis, and rotation posture around the y-axis of the unmanned ship), the six-degree-of-freedom model is simplified to a three-degree-of-freedom model. , ; The kinematic model of the unmanned ship is as follows:

[0043] ;

[0044] Assuming that the unmanned ship is a rigid body with uniform mass distribution, the structure of the ship is bilaterally symmetrical, and it has corresponding hydrodynamic coefficients, the dynamic model of the unmanned ship can be described as follows:

[0045] ,

[0046] in, , Indicates the quality of the unmanned ship, represents the hydrodynamic parameters of the unmanned ship, represents the rotation torque, represents the hydrodynamic damping parameter of the unmanned ship; Indicates the driving force of the unmanned boat's forward direction. It indicates that the outside world interferes with the unmanned ship's forward direction. Indicates the interference and obstruction of the lateral direction of the unmanned boat by the outside world. Indicates the driving force that drives the unmanned boat to turn. Indicates the interference and obstruction of the unmanned ship's turning direction by the outside world;

[0047] In summary, under ideal conditions, the kinematic and dynamic models of unmanned ships without external interference are described as follows:

[0048] .

[0049] The specific process of step (2) is:

[0050] In the shepherding algorithm, the net force on the sheep is denoted by , the combined force on the sheepdog is recorded as , provides the movement direction for the sheepdog and sheep, and drives the sheep and sheepdog to move in the movement direction through the added movement speed. The movement position is recorded as (the position of the sheep and the sheepdog at the next moment), the moving speed is recorded as (the speed of the sheep and the sheepdog at the next moment), the actual position of the unmanned boat at the current moment is recorded as , through posture calculation, calculate the desired steering angle ;

[0051] Increase the movement speed of sheep and sheepdogs As the expected speed, the expected steering angle and expected speed Actual angle with the unmanned ship and speed Subtract and get the steering angle error Error with moving speed , as the input of the ADRC controller, that is:

[0052] ,

[0053] in, and The actual turning angle (i.e., bow pitch angle) and moving speed (i.e., the speed of the unmanned ship toward the target) of the unmanned ship are obtained by calculating the kinematic and dynamic models of the unmanned ship in step (1). direction of movement);

[0054] According to the input steering angle error Error with moving speed , the forward thrust required for the unmanned ship to move is obtained through the adjustment of the ADRC controller and steering thrust (These two parameters appear in the dynamic model of the unmanned ship). The movement of the unmanned ship is realized through the kinematic and dynamic models of the unmanned ship in step (1).

[0055] The process of posture calculation is as follows: the moving position calculated by the shepherding algorithm is the ideal position and the actual position of the unmanned ship Do the difference and get the vector , this vector is related to the attached coordinate system of the unmanned ship The angle between the positive and negative axes is the desired steering angle. ,Right now:

[0056] ,

[0057] In the formula Represents the attached coordinate system The positive direction of the axis.

[0058] The forward thrust required for the unmanned boat to move is obtained by adjusting the ADRC controller. and steering thrust , the specific process is:

[0059] The ADRC controller includes a tracking differentiator, nonlinear state error feedback, an extended state observer and a disturbance compensation link;

[0060] ① Tracking differentiator;

[0061] In order to extract and estimate high-order derivatives from the input signal and help the extended state observer obtain accurate state information, a discrete tracking differentiator is designed as follows:

[0062] ,

[0063] in, represents the input signal, represents the first-order derivative of the input signal, represents the second-order derivative of the input signal, The optimal speed control comprehensive function of the second-order discrete system is represented by:

[0064] ,

[0065] In the formula, is the desired turning angle of the unmanned ship, is the expected speed of the unmanned ship, is the filtering step length, which is used to suppress the influence of noise interference; is the filter sampling factor, After confirming, take , control overshoot and suppress noise amplification; is the speed factor. As the speed factor increases, The speed of tracking the input signal is increasing, and when it reaches a certain level, Considered as the differential form of the input signal;

[0066] ② Extended state observer;

[0067] According to the formula of the unmanned ship kinematics and dynamics model in step (1), let , the nonlinear part of the unmanned ship kinematics and dynamics model is regarded as a disturbance and , so the formula of the unmanned ship kinematics and dynamics model is rewritten as:

[0068] ,

[0069] when and When is known, the extended state observer is assumed to be:

[0070] ,

[0071] exist and In the unknown case, the unknown variable and Expand to new state variables and ,Right now:

[0072] ;

[0073] make , substitute into the rewritten unmanned ship kinematic and dynamic model formula (see formula (29) in the specific implementation method), and expand to obtain a new linear system:

[0074] ;

[0075] By analyzing the new linear system, we get the new extended state observer:

[0076] ,

[0077] in, represents the observer parameters, represents the output variable of the extended state observer, Represents the new state variable and (See formula (31) in the specific implementation) of the observed value, The specific form of the function is as follows:

[0078] ;

[0079] in, express The linear interval width of the function, represents the parameters of the extended state observer and , generally take , ; For parameters , when appropriate parameter values ​​are chosen, each state variable can be well estimated:

[0080] ;

[0081] ③Nonlinear state error feedback;

[0082] In order to stabilize the formulas of the USV kinematic and dynamic models and reduce the error, the nonlinear state error feedback control rate form is adopted:

[0083] ;

[0084] In the formula, Indicates the response speed parameter of ADRC controller, and Making it bigger will increase the response speed. Increasing it will inhibit the response speed; express The linear interval width of the function; generally choose the parameter , This can greatly improve the control accuracy and reduce the error. , and error tracking signal Satisfies the following relationship:

[0085] ;

[0086] ④ Disturbance compensation;

[0087] In the extended state observer, the nonlinear part of the unmanned ship model is equivalent to the disturbance After estimation, the control input of the unmanned ship is finally obtained by compensating the total disturbance:

[0088] ;

[0089] Substituting the above equation into the rewritten USV kinematic and dynamic model formula (see equation (29) in the specific implementation method), we get:

[0090] ;

[0091] Also because , so the above formula is simplified to:

[0092] ;

[0093] In this way, the nonlinear formula of the rewritten unmanned ship kinematics and dynamics model is transformed into a linear form, thereby obtaining the forward thrust required by the unmanned ship model. and steering thrust .

[0094] The present invention combines the Shepherding algorithm with the ADRC controller to obtain the forward thrust and steering thrust of the unmanned boat, thereby realizing the movement of the unmanned boat, and dividing the unmanned boats into our USVs and enemy USVs. The Shepherding algorithm can control our USVs to encircle and capture the enemy USVs. During the encirclement and capture process, the enemy USVs will escape from the encirclement and perform confrontational movement, and will eventually be driven to the target area; during the movement of the unmanned boat, the distance between the enemy and our unmanned boats is well maintained, which can effectively avoid collisions; at the same time, sufficient forward and steering thrusts can be calculated for the movement of the unmanned boat, which can meet the needs of the encirclement and capture confrontation process. BRIEF DESCRIPTION OF THE DRAWINGS

[0095] Figure 1 It is a schematic diagram of two coordinate systems when the unmanned ship moves on the sea.

[0096] Figure 2 It is a two-dimensional plan schematic diagram of the unmanned ship.

[0097] Figure 3 This is a schematic diagram of a sheepdog performing a driving task.

[0098] Figure 4 This is a schematic diagram of a sheepdog performing a collecting task.

[0099] Figure 5 It is a structural block diagram of the unmanned boat encirclement and countermeasure method based on shepherding algorithm of the present invention.

[0100] Figure 6 This is a schematic diagram of the movement trajectory of the unmanned boat encirclement and confrontation.

[0101] Figure 7 This is a schematic diagram of the X-axis change during the unmanned boat capture and countermeasure movement process

[0102] Figure 8 It is a schematic diagram of the y-axis changes during the unmanned boat encirclement and confrontation movement process.

[0103] Fig. 9 This is a schematic diagram of the change in the position of an unmanned ship (USV) due to the buoyancy.

[0104] Fig.10 It is a schematic diagram of the steering angle error of an unmanned vehicle (USV).

[0105] Fig.11 is the forward speed of the unmanned ship (USV) Schematic diagram.

[0106] Fig.12 Is the unmanned ship (USV) forward thrust Schematic diagram of the change curve.

[0107] Fig.13 Is the steering thrust of the unmanned ship (USV) Schematic diagram of the change curve.

[0108] Fig.14 This is a schematic diagram of an unmanned boat moving and maintaining distance. DETAILED DESCRIPTION

[0109] The unmanned boat encirclement and countermeasure method based on shepherding algorithm of the present invention is described in detail below with reference to the accompanying drawings.

[0110] 1. Establish the kinematic and dynamic model of USV (unmanned vehicle);

[0111] For the study of the motion of unmanned ships at sea, the kinematic model and dynamic model are the core parts.

[0112] 1. USV kinematic model;

[0113] When an unmanned ship moves on the sea, it usually moves in six degrees of freedom. For this purpose, two coordinate systems are generally established: the geodetic coordinate system fixed on the ground and the attached coordinate system fixed to the hull ,like Figure 1 shown.

[0114] Geodetic coordinate system Generally used to describe the location information of unmanned ships, the origin of the coordinate system Anywhere on the sea surface, The axis points to the true north of the earth, which is generally defined as the positive direction of the unmanned ship's movement. The axis points to the east of the earth, which is to the right of the direction of movement of the unmanned ship. Axis perpendicular to sea level The plane points downward, which is the direction in which the unmanned boat moves up and down.

[0115] Attached coordinate system It is generally used to describe the attitude information of unmanned ships. The origin of the coordinate system is generally located at the center of gravity of the unmanned ship. The axis is located on the centerline of the hull and points to the front of the hull. The axis is perpendicular to the centerline of the hull and points to the right side of the hull. Perpendicular to The flat surface points toward the underside of the hull.

[0116] The parameters related to the earth coordinate system and the attached coordinate system are shown in Table 1 and Table 2. The position of the unmanned ship in the ocean is Coordinates are used to represent the posture during movement, and the heel angle is used to represent the posture during movement. , pitch angle , Bow Angle To express, that is Indicates the attitude information of the unmanned ship, The speed of the direction of movement is Indicates, around The angular velocity of the direction of rotation is Indicates that towards The speed of the direction of movement is Indicates, around The angular velocity of the direction of rotation is Indicates that towards The speed of the direction of movement is Indicates, around The angular velocity of the direction of rotation is express.

[0117] Table 1 Geodetic coordinate system USV parameter information

[0118]

[0119] Table 2 Attached body coordinate system middle Parameter information

[0120]

[0121] The two-dimensional plane of the unmanned ship is as follows Figure 2 shown. , They respectively represent the spatial position coordinates and attitude angle information of the USV in the geodetic coordinate system. , They represent the linear velocity information and angular velocity information of the USV in the attached coordinate system respectively. Therefore, the state change of the USV moving in space can be expressed as:

[0122] . (1)

[0123] Geodetic coordinate system O - XYZ With the attached coordinate system The relationship between position and speed is converted as follows:

[0124] ; (2)

[0125] in, The matrix representation is as follows:

[0126] . (3)

[0127] Geodetic coordinate system O - XYZ With the attached coordinate system The relationship between attitude and angular velocity is converted as follows:

[0128] , (4)

[0129] in, The matrix representation is as follows:

[0130] . (5)

[0131] The motion model of USV can be expressed as follows:

[0132] , (6)

[0133] in, The matrix is ​​represented as:

[0134] . (7)

[0135] From the USV motion analysis, it can be seen that in the actual operation process, The movement posture of the USV is expressed in six degrees of freedom. However, in the present invention, only the movement posture of the USV on the same plane is considered, that is, the up and down movement, the rotation posture around the x-axis and the rotation posture around the y-axis of the USV are not considered. The six-degree-of-freedom model can be simplified to a three-degree-of-freedom model. At this time, , , Equation (6) can be rewritten as a three-degree-of-freedom model as follows:

[0136] . (8)

[0137] 2. USV dynamics model;

[0138] Assuming that the USV is a rigid body with uniform mass distribution, the structure of the ship is bilaterally symmetrical, and has corresponding hydrodynamic coefficients, the dynamic equation of the unmanned ship moving on the sea surface can be described as:

[0139] (9)

[0140] in The matrix is ​​the mass matrix of USV, The matrix represents the Coriolis centripetal force coefficient matrix, The matrix represents the hydrodynamic damping coefficient matrix, and the specific expression is as follows:

[0141] , (10)

[0142] , (11)

[0143] , (12)

[0144] in, , Indicates the quality of the unmanned ship, represents the hydrodynamic parameters of the unmanned ship, represents the rotation torque, represents the hydrodynamic damping parameter of the unmanned ship; Indicates the driving force of the unmanned boat's forward direction. It indicates that the outside world interferes with the unmanned ship's forward direction. Indicates the interference and obstruction of the lateral direction of the unmanned boat by the outside world. Indicates the driving force that drives the unmanned boat to turn. It indicates the interference and obstruction of the steering direction of the unmanned boat by the outside world. represents the driving force matrix of the system, Represents the external interference matrix caused to the system, and the expression is as follows:

[0145] , (13)

[0146] . (14)

[0147] exist In the matrix, Indicates the driving force of the unmanned ship's forward direction; Indicates the driving force of the lateral movement of the unmanned ship. Since there is no lateral driving force in the unmanned ship, ; Indicates the driving force that drives the USV to turn. The driving force matrix Further simplified to:

[0148] . (15)

[0149] exist In the matrix, Indicates the interference and obstruction of the USV's forward direction from the outside world. Indicates the interference and obstruction of the external environment to the lateral direction of the USV. Indicates external interference and obstruction to the USV's steering direction.

[0150] The dynamic model of USV is described as:

[0151] . (16)

[0152] In summary, under ideal conditions, the kinematic and dynamic models of USV without external interference can be described as:

[0153] . (17)

[0154] 2. Shepherding algorithm;

[0155] The shepherding algorithm is a biologically inspired crowd control method based on the herding behavior of sheepdogs. This method is not only applicable to herding, but also to the control of multi-agents. The shepherding problem refers to one or more sheepdogs driving a sheepfold consisting of sheep to a target area, which is mainly divided into the behavior of sheep and the behavior of sheepdogs.

[0156] use represents the set of sheep, where Indicates the number of sheep.

[0157] use represents a set of sheepdogs, where Indicates the number of sheepdogs.

[0158] use Denotes the behavior of a sheepdog or sheep. Indicates the number of corresponding behaviors.

[0159] 1. The behavior of sheep;

[0160] When sheep are moving, they will try to stay away from the sheepdog to avoid interference from the sheepdog, which is mainly affected by six behaviors:

[0161] (1) Any sheep will be driven away by the sheepdog approaching the flock, that is, it will be rejected by the sheepdog;

[0162] (2) Each sheep will be attracted by the center of the flock and move closer to the center area (center of mass) of the flock. That is, each sheep will be attracted by the center area.

[0163] (3) Each sheep must avoid colliding with other sheep;

[0164] (4) There is random movement behavior in the flock;

[0165] (5) The sheep will be affected by the direction they moved in the previous moment, that is, by their own inertia;

[0166] (6) Sheep should avoid colliding with obstacles.

[0167] Among them, the purpose of the fourth behavior is to prevent the sheep from being stuck in a local movement and unable to jump out during the movement, thereby ensuring that the sheep can move smoothly all the time.

[0168] Any sheep will try to escape from the sheepdog, the farther away from the sheepdog the better. The distance that the sheep can sense the sheepdog is recorded as , the repulsion of sheep to sheepdogs The calculation method is calculated using the following formula:

[0169] , (18)

[0170] in, The ratio of the speed of the sheepdog to the speed of the sheep. represents the distance between the sheep and the sheepdog, Indicates the maximum distance at which the sheepdog will affect the sheep. Indicates the minimum distance that must be maintained between sheep and sheepdogs to avoid collisions between them.

[0171] In the sensing range Each sheep within is detected and considered as a neighbor for calculating the centroid of the central region and thus the cohesion towards the local centroid. , indicating that Moment The sheep are subject to cohesion.

[0172] In order to avoid collisions between sheep, a sheep collision avoidance force , indicating that Moment The collision avoidance force on the sheep is is a repulsive force and is calculated using the following formula:

[0173] , (19)

[0174] Among them, the parameters Indicates the number of sheep.

[0175] When there is no other behavior in the flock, random jitter is introduced to avoid the flock from being trapped in local movement and unable to jump out. , indicating that Moment The position of the sheep changes randomly. Makes small random changes to the sheep's position coordinates to randomly affect the sheep's position.

[0176] In order to more realistically reproduce the situation of sheep in real life, inertia force is introduced , indicating that Moment The sheep will be affected by the previous moment The influence of the moving direction.

[0177] In reality, there are always obstacles. In order to prevent the sheep from colliding with obstacles during movement, collision avoidance force is introduced. , indicating that Moment The collision avoidance force on the sheep is detected when the sheep is moving. When there is an obstacle within range, the sheep will avoid it like a sheepdog, and the calculation formula is as follows:

[0178] , (20)

[0179] Among them, since the obstacle is stationary, the parameter Indicates the moving speed of the sheep. Indicates the detection radius of the sheep to the obstacle , Indicates the minimum distance that must be maintained between the sheep and the obstacle to avoid collision between the sheep and the obstacle.

[0180] The next movement of a sheep will be affected by the above six behaviors. The total force affecting the next position of the sheep is composed of the weighted form of these six behavioral forces. The weighted force is as follows:

[0181] ,(twenty one)

[0182] in, represents the net force on the sheep; It means that the sheep is influenced by the direction of movement at the previous moment, that is, the inertial force; It represents the attraction of the flock centroid to the sheep; It represents the repulsion of sheep to sheepdogs; Indicates the collision avoidance force to prevent sheep from colliding with each other; It means the random jitter force that prevents the flock from falling into local movement and being unable to jump out; Indicates the collision avoidance force to avoid collision with obstacles. They represent the influence intensity of the corresponding behavior respectively. The larger the parameter value, the higher the influence intensity, and the smaller the value, the lower the intensity.

[0183] 2. Shepherd dog behavior;

[0184] The task of a sheepdog is to drive the sheep into the sheepfold. When a sheep escapes from the flock, the sheepdog will drive the sheep back to the flock. Therefore, the sheepdog has two main tasks: driving and collecting. The driving and collecting points of the sheepdog when performing the task are as follows: Figure 3 and Figure 4 shown.

[0185] When the sheep gather together, that is, they are all within the range of the flock, the sheepdog performs the driving task. The sheepdog moves around the center of mass of the flock and moves to the driving point. The driving point is on the line connecting the center of mass of the flock and the sheepfold (the designated target area Goal), and the distance from the center of mass is The range is towards the target position, that is Figure 3 middle Location point. Indicates the radius of the flock, that is, when a sheep exceeds the distance from the flock centroid When a sheep is out of range, it is considered that the sheep has left the flock, and the sheepdog is needed to perform the collection task and drive the sheep that have left the flock back to the flock; Indicates the minimum distance a sheepdog should maintain from a sheep while moving. Indicates that the shepherd dog is moving around the center of mass of the flock The distance at which moving on a circle of radius will not have any effect on sheep.

[0186] When a sheep escapes from the flock, the sheepdog will perform the collection task. The sheepdog will identify the sheep farthest from the centroid of the flock and move to the collection point to drive the sheep away from the flock back to the flock. The collection point is located on the line connecting the centroid of the flock and the position of the farthest sheep. The range is towards the centroid of the flock, that is, Figure 4 middle Location point.

[0187] During the movement of the sheepdog, in order to prevent the sheepdog from being trapped in local movement and unable to jump out, it is necessary to add random jitter to the movement of the sheepdog. , the force pointing to the driving point or the collecting point is recorded as , so the net force on the sheepdog is as follows:

[0188] ,(twenty two)

[0189] In the formula, Indicates A shepherd dog When the sheepdog moves, when it approaches the driving point or the collecting point, it will circle around the center of mass of the flock. The sheepdog moves in a circular trajectory with a radius of 2.5 and keeps approaching the driving point or the collecting point, which ensures that the sheepdog will not affect the sheep during the movement.

[0190] To calculate the minimum radius of a flock that can accommodate all the sheep, the minimum radius is To calculate, the expression is as follows:

[0191] ,(twenty three)

[0192] In the formula, represents the distance between two sheep to prevent collision. Indicates the number of sheep.

[0193] When the sheepdog is performing its task, the sheep generate repulsion to avoid the pursuit of the sheepdog. , the repulsive force can be expressed as The calculation formula is further specified as:

[0194] ,(twenty four)

[0195] So the sheep are away from the sheepdog The repulsive force is greatest at a distance of When the sheepdog is performing the collection task, the sheep are at a distance of However, in the driving task, the distance between the sheep and the driving point of the sheepdog may be greater than , so that the sheepdog has no influence on the driving task of the sheep, which will affect the realization of the driving task. In order to make the driving task of the sheepdog affect at least one sheep in the sheepfold, the following assumptions are made: ,Right now ,in .

[0196] In the shepherding algorithm, the net force on the sheep is , the net force on the sheepdog is , provides the movement direction for the sheepdog and sheep, and drives the sheep and sheepdog to move in the movement direction through the added movement speed. The movement position is recorded as , which is the position of the sheep and the sheepdog at the next moment, and the moving speed is recorded as , which is the speed of the sheep and the sheepdog at the next moment, and the actual position of the unmanned boat at the current moment is recorded as , the actual angle of the unmanned ship is , the actual speed of the unmanned ship is , and The actual turning angle (i.e., bow pitch angle) and moving speed (i.e., the speed of the unmanned ship toward the target) of the unmanned ship are obtained by calculating the kinematic and dynamic models of the unmanned ship in step (1). The speed of movement in the direction), through posture solution, the desired steering angle is solved The process of posture solution is as follows: The moving position calculated by the shepherding algorithm is the ideal position and the actual position of the unmanned ship Make a difference and get the ideal position vector , this vector is related to the attached coordinate system of the unmanned ship The angle between the positive and negative axes is the desired steering angle. ,Right now:

[0197] , (25)

[0198] In the formula Represents the attached coordinate system The positive direction of the axis.

[0199] Increase the movement speed of sheep and sheepdogs As the expected speed, the expected steering angle and expected speed Actual angle with the unmanned ship and speed Subtract and get the steering angle error Error with moving speed , as the input of the ADRC controller, that is:

[0200] , (26)

[0201] According to the input steering angle error Error with moving speed , the forward thrust required for the unmanned ship to move is obtained through the adjustment of the ADRC controller and steering thrust , the movement of the unmanned ship is realized through the kinematic and dynamic model of the unmanned ship.

[0202] Obtain the forward thrust required for the unmanned ship to move and steering thrust The process is described in detail in the third part below. 3. Active Disturbance Rejection Control ADRC;

[0203] The ADRC controller mainly consists of four parts: Tracking Differentiator (TD), Nonlinear State Error Feedback (NLSEF), Extended State Observer (ESO) and disturbance compensation link.

[0204] 1. Tracking differentiator TD;

[0205] In order to extract and estimate high-order derivatives from the input signal and help the extended state observer obtain accurate state information, a discrete tracking differentiator is designed as follows:

[0206] , (27)

[0207] in, represents the input signal, represents the first-order derivative of the input signal, represents the second-order derivative of the input signal, The optimal speed control comprehensive function of the second-order discrete system is represented by:

[0208] , (28)

[0209] In the formula, is the desired turning angle of the unmanned ship, is the expected speed of the unmanned ship, and the parameters It is called the filtering step size and is used to suppress the influence of noise interference; It is called the filter sampling factor. After confirmation, generally take , which can effectively control overshoot and suppress noise amplification. is called the speed factor. As the speed factor increases, The speed of tracking the input signal is increasing, and when it reaches a certain level, it can is considered as the differential form of the input signal.

[0210] 2. Extended State Observer ESO;

[0211] According to the USV kinematics and dynamics model (17), let , the nonlinear part of the model is considered as a perturbation and , so the USV kinematic and dynamic model (17) can be rewritten as:

[0212] . (29)

[0213] when and When is known, the extended state observer can be set as:

[0214] . (30)

[0215] But in reality, and Almost all of them are unknown. In this case, the unknown variables can be and Expand to new state variables and ,Right now:

[0216] . (31)

[0217] make , substituting into formula (29) can expand to obtain a new linear system:

[0218] . (32)

[0219] By analyzing the new linear system, we get the new extended state observer:

[0220] , (33)

[0221] in, represents the observer parameters, represents the output variable of the extended state observer, The observed value of expression (31) is The specific form of the function is as follows:

[0222] . (34)

[0223] in, express The linear interval width of the function, represents the parameters of the extended state observer and , generally take , ; For parameters , when appropriate parameter values ​​are chosen, each state variable can be well estimated:

[0224] . (35)

[0225] 3. Nonlinear state error feedback NLSEF;

[0226] In NLSEF, in order to stabilize the USV kinematic and dynamic model (17) and reduce the error, the nonlinear state error feedback control rate can be adopted:

[0227] , (36)

[0228] In the formula, Indicates the response speed parameter of ADRC controller, , Making it bigger will increase the response speed. Increasing it will inhibit the response speed; express The linear interval width of the function; generally choose the parameter , , which can greatly improve the control accuracy and reduce the error. , and error tracking signal Satisfies the following relationship:

[0229] . (37)

[0230] 4. Disturbance compensation;

[0231] In the extended state observer, the nonlinear part of the unmanned ship model is equivalent to the disturbance After estimation, the control input of the unmanned ship is finally obtained by compensating the total disturbance:

[0232] . (38)

[0233] Substituting the above formula (38) into the rewritten formula (29) of the USV kinematic and dynamic model, we can obtain: . (39)

[0234] Also because , so the above formula (39) can be simplified as:

[0235] . (40)

[0236] In this way, the nonlinearity of model equation (29) can be transformed into a linear form. Thus, the forward thrust required by the unmanned ship model is obtained: and steering thrust .

[0237] 4. USV encirclement and confrontation based on Shepherding algorithm;

[0238] The present invention is based on the USV roundup confrontation of the Shepherding algorithm, and requires that the roundup and confrontation be realized through the algorithm. First, it is necessary to confirm the heading control and steering control of the USV during movement, so it is necessary to design a forward speed controller and a steering heading controller to realize the forward and steering control of the USV. The position and speed of the sheep and the sheepdog at the next moment are calculated by the Shepherding algorithm, and the obtained position information is subtracted from the position information of the USV at the current moment to obtain the expected steering angle. The speed of the sheep and the sheepdog is regarded as the expected speed. The expected steering angle and the expected speed are subtracted from the actual angle and speed of the USV to obtain the steering angle error and the moving speed error, thereby calculating the forward thrust required by the controller. and steering thrust The designed structure diagram is as follows Figure 5 shown.

[0239] In the shepherding algorithm, both the sheep and the sheepdog are affected by various behavioral forces, and the combined force on the sheep is recorded as , the combined force on the sheepdog is recorded as , provides the movement direction for the sheepdog and the sheep, and drives the sheep and the sheepdog to move accordingly through the added movement speed. The moving position is recorded as , the speed of movement is recorded as ; Through posture calculation, the desired steering angle can be calculated , so the input of the ADRC controller is

[0240] (47)

[0241] in, , represents the actual steering angle and forward speed of the unmanned ship obtained by equation (17) represents the pitch angle, Indicates that the unmanned ship is heading towards direction of movement).

[0242] Through the adjustment of the ADRC controller, the forward thrust required for the unmanned ship to move can be obtained. and steering thrust , the movement of the unmanned boat can be realized by formula (17).

[0243] 5. Simulation experiment;

[0244] In order to verify the effectiveness of the designed control system, the control situation is simulated and verified. In order to study the confrontation of the encirclement, the unmanned ships are divided into our USV and the enemy USV. The sheepdog algorithm guides our unmanned ship to encircle, and the sheep algorithm guides the enemy unmanned ship to fight and escape. The encirclement confrontation between one USV and one enemy USV is studied. The coordinates of the encirclement target point Goal are set to (88, 90). When the enemy USV is driven within 20 of the target area, it is considered that the encirclement is successful and the encirclement process is over.

[0245] Set the initial state of our USV: initial position is (8, 8), initial speed , initial angular velocity , initial angle , initial forward thrust , initial steering thrust , the initial distance from the enemy USV is 100.

[0246] Set the initial state of the enemy USV: the initial position is (8, 108), the initial speed , initial angular velocity , initial angle , initial forward thrust , initial steering thrust , the initial distance from the target point Goal is 82.

[0247] Depend on Figure 6 , Figure 7 , Figure 8 It can be seen that the shepherding algorithm can guide our unmanned ships to encircle and capture enemy unmanned ships and drive them to the target area. When the enemy unmanned ships are driven within 20 range of the target area, the encirclement process ends. Fig. 9 It shows the position change caused by the buoyancy of the unmanned boat, reflecting the three-dimensional effect.

[0248] exist Fig.10 In the process, the angle error of our unmanned ship is relatively smooth. When it reaches about 1700 degrees during the capture process, the angle error increases. Our unmanned ship moves to the vicinity of the enemy's unmanned ship. In order to avoid collision, the angle change increases. Fig.11 The curve of the change in the unmanned ship's forward speed u reflects this point, and the collision avoidance effect is good; when the enemy's unmanned ship reaches about 600 in the process of encirclement and capture, the speed error decreases. This is because it is being encircled by our unmanned ships, and in order to escape the encirclement and capture, it increases its forward speed, reflecting its confrontation. Fig.10 In the simulation, the steering angle error trajectories of our unmanned ship and the enemy unmanned ship are relatively consistent, and the escape angle of the enemy unmanned ship can be well identified by our unmanned ship and effectively tracked. Fig.11 , Fig.12 , Fig.13 It can be seen that the controller can effectively control the input signal and obtain the corresponding output, thereby controlling the movement of the unmanned ship.

[0249] Depend on Fig.14 It can be seen that in the initial state, the distance between the two unmanned ships is 100, and the distance between the enemy unmanned ship and the target point is 82. When the encirclement process reaches about 1000, the enemy unmanned ship escapes from the target area at the maximum distance, and then begins to be driven to the target position. At about 2500 in the encirclement process, it is driven to within 20 of the target area, and the encirclement process ends; at the same time, the distance between our unmanned ship and the enemy unmanned ship is maintained at more than 20, which effectively avoids collisions and has a good collision avoidance effect.

[0250] Through simulation results, it is found that the Shepherding algorithm can well guide our USV to encircle the enemy USV, while the enemy USV will escape from the encirclement during the encirclement process, which is very confrontational; during the movement of the unmanned ship, the distance between our USV and the enemy USV is more than 20, which can effectively avoid collision; the designed USV controller can effectively control the forward movement and turning movement of the USV, which can meet the needs of encirclement and confrontation.

Claims

1. A method for unmanned boat encirclement and confrontation based on shepherding algorithm, characterized in that: The following steps are involved: (1) Establish the kinematic and dynamic models of the unmanned ship; (2) The unmanned ship is controlled by the Shepherding algorithm. The Shepherding algorithm is combined with the ADRC controller to obtain the forward thrust and steering thrust of the unmanned ship, thereby realizing the movement of the unmanned ship. The ADRC controller includes a tracking differentiator, a nonlinear state error feedback, an extended state observer, and a disturbance compensation link. (3) The unmanned ships are divided into friendly unmanned ships and enemy unmanned ships. The friendly unmanned ships are used as sheepdogs and the enemy unmanned ships are used as sheep. The unmanned ship encirclement and confrontation control is realized through step (2); The specific process of step (2) is: In the shepherding algorithm, the net force on the sheep is denoted by F π The total force on the sheepdog is denoted as F β , provides the movement direction for the sheepdog and sheep, and drives the sheep and sheepdog to move in the movement direction through the added movement speed. The movement position is recorded as S p , the moving speed is recorded as S u , the actual position of the unmanned ship at the current moment is recorded as S c , through posture calculation, the desired steering angle ψ is calculated p ; Increase the movement speed of sheep and sheepdogs to S u As the desired speed, the desired steering angle ψ p and the desired speed S u Differ from the actual angle ψ and speed u of the unmanned ship to obtain the steering angle error e ψ The error of moving speed e u , as the input of the ADRC controller, that is: Wherein, ψ and u are the actual steering angle and moving speed of the unmanned ship obtained by calculating the kinematic and dynamic models of the unmanned ship in step (1); According to the input steering angle error e ψ The error of moving speed e u , the forward thrust τ required for the unmanned ship to move is obtained by adjusting the ADRC controller Fu and steering thrust τ Fr , the movement of the unmanned ship is realized through the kinematic and dynamic models of the unmanned ship in step (1); The process of posture solution is as follows: The mobile position S calculated by the shepherding algorithm p is the ideal position, and the actual position S of the unmanned ship c Do the difference and get the vector The angle between this vector and the positive direction of the ox axis of the unmanned ship's attached coordinate system is the desired steering angle ψ p ,Right now: In the formula Indicates the positive direction of the ox axis of the attached coordinate system.

2. The unmanned boat encirclement and countermeasure method based on shepherding algorithm according to claim 1 is characterized in that: The specific process of establishing the kinematic and dynamic models of the unmanned ship in step (1) is as follows: When the unmanned ship moves on the sea, it moves with six degrees of freedom. A geodetic coordinate system O-XYZ fixed on the ground and an attached coordinate system o-xyz fixed on the hull are established; the geodetic coordinate system O-XYZ is used to describe the position information of the unmanned ship. The origin O of the coordinate system is located at any position on the sea surface. The OX axis points to the north direction of the earth, which is defined as the positive direction of the unmanned ship's movement. The OY axis points to the east direction of the earth, which is the right side of the unmanned ship's movement direction. The OZ axis is perpendicular to the sea level XOY plane and points downward, which is the up and down movement direction of the unmanned ship; the attached coordinate system o-xyz is used to describe the posture information of the unmanned ship. The origin of the coordinate system is located at the center of gravity of the unmanned ship. The ox axis is located on the central axis of the hull and points to the front of the hull. The oy axis is perpendicular to the central axis of the hull and points to the right side of the hull. The oz is perpendicular to the xoy plane and points to the lower side of the hull. The position of the unmanned ship in the ocean is represented by (x, y, z) coordinates, and the posture during movement is represented by the heel angle. It is expressed by the pitch angle θ and the bow angle ψ, that is, Indicates the attitude information of the unmanned ship, the speed moving in the x direction is represented by u, the angular velocity rotating around the x direction is represented by p, the speed moving in the y direction is represented by v, the angular velocity rotating around the y direction is represented by q, the speed moving in the z direction is represented by w, and the angular velocity rotating around the z direction is represented by r; Considering only the movement posture of the unmanned ship on the same plane, the six-degree-of-freedom model is simplified to a three-degree-of-freedom model. At this time, let w = p = q = 0. The kinematic model of the unmanned ship is as follows: Assuming that the unmanned ship is a rigid body with uniform mass distribution, the structure of the ship is bilaterally symmetrical, and it has corresponding hydrodynamic coefficients, the dynamic model of the unmanned ship can be described as follows: in, m represents the mass of the unmanned ship, represents the hydrodynamic parameters of the unmanned ship, I Z represents the rotational moment, d 11 ,d 22 ,d 33 represents the hydrodynamic damping parameter of the unmanned ship; τ Fu Indicates the driving force of the unmanned ship in the forward direction, τ Du Indicates the interference and obstruction of the external environment to the unmanned ship's forward direction, τ Dv represents the interference from the outside world to the lateral direction of the unmanned ship, τ Fr represents the driving force for the unmanned boat to turn, τ Dr Indicates the interference and obstruction of the unmanned ship's turning direction by the outside world; In summary, under ideal conditions, the kinematic and dynamic models of unmanned ships without external interference are described as follows:

Images