Multi-AUV formation reconstruction obstacle avoidance control method based on artificial potential field and affine transformation

Abstract

The invention relates to the technical field of obstacle avoidance control, and discloses a multi-AUV formation reconstruction obstacle avoidance control method based on an artificial potential field and affine transformation, and the method comprises the steps: construction of an improved artificial potential field function, and design of a multi-AUV formation reconstruction obstacle avoidance controller based on affine transformation. According to the method, the artificial potential field is improved, so that the problem that in a traditional artificial potential field method, the existing potential field cannot be reached in a target area is effectively solved. The strong deformation capability of affine transformation and the efficient obstacle avoidance efficiency of the IAPF are effectively combined for the first time, the transformation obstacle avoidance modes based on the affine transformation during formation contraction, formation displacement, formation shearing, formation transfer and the like are provided, and formation transformation selection is provided for multi-AUV formation overall obstacle avoidance.

Description

Technical Field

[0001] The present invention relates to the technical field of obstacle avoidance control in formation control of multiple AUVs, and in particular to a multi-AUV formation reconstruction obstacle avoidance control method based on artificial potential field and affine transformation. Background Art

[0002] For land unmanned vehicles and aerial unmanned vehicles, the obstacle avoidance algorithms and path planning of single robots have reached a relatively mature level, and many classic obstacle avoidance path planning and optimization algorithms have been derived. Many methods can plan a safe navigation path from the starting point to the target point offline or online. However, for AUVs, the underwater environment is usually complex and dynamic and unknown. It is difficult to study the obstacle avoidance control and navigation path planning of AUVs. The advantage is that it is a considerable challenge to apply the algorithm to practice. Despite this, scholars have conducted serious research and worked hard to solve the problem, and have made quite forward-looking research results in AUV obstacle avoidance and path planning. But relatively speaking, there is less research on obstacle avoidance of multiple AUV formations. In AUV formation control, in certain specific navigation tasks, such as formation escort and coordinated transportation, formation AUVs need to avoid static and dynamic obstacle threats while maintaining the formation. This requires the multi-AUV system to be able to cope with complex obstacle environments and plan navigable paths while maintaining the existing formation or reconstructing the formation. However, most of the current research on multi-AUV formation control considers static obstacles or single dynamic obstacles alone, or when encountering obstacles during formation navigation, the formation is generally decomposed and reorganized after avoiding obstacles. Obviously, due to the particularity of the underwater environment, the decomposition and reorganization method will face many problems in practical applications, such as communication interruption, loss of individual AUVs, etc., and the risk is greater. In addition, compared with two-dimensional routes, three-dimensional routes can give full play to the maneuverability of AUVs, thereby adapting to underwater detection, target interception, formation escort and other task planning. Therefore, dynamic formation reconstruction and obstacle avoidance technology in complex three-dimensional environments have received more and more attention. In most actual underwater engineering tasks, there are various obstacles (static and dynamic) in the AUV navigation environment, and the formation AUVs need to avoid these obstacles. In addition, the paths of multi-AUV formation navigation are mostly unknown. Obstacle avoidance and route planning in complex underwater environments are hot issues in the current AUV formation research. It requires the formation AUVs to be able to autonomously plan an obstacle avoidance path suitable for their own formation structure navigation in complex underwater environments, avoid static obstacles or dynamic obstacles in the environment, and complete the navigation mission. However, in the case that the obstacle avoidance navigation control of a single AUV is already very difficult, the coordination of multiple AUVs makes the control difficulty of multiple AUVs no longer increase exponentially, which means that the design of the obstacle avoidance controller for a multi-AUV formation is even more difficult.

[0003] The obstacle avoidance problem is very critical in the formation control of multiple AUVs, and has become an extremely important link in the process of AUVs completing underwater operation tasks. The obstacle avoidance methods of AUVs mainly include artificial potential field method, template matching method, map construction and intelligent optimization method, etc. Among them, the artificial potential field method is widely used because of its simple principle, rapid response, easy implementation and expansion. It is a representative method of robot path planning. However, in some cases, the traditional artificial potential field method has the problem of unreachable targets, that is, when the gravity of the target area, the repulsion of obstacles and the combined force of neighboring AUVs on the AUV are zero, the underwater robot group will fail to avoid obstacles. Summary of the invention

[0004] The purpose of the present invention is to provide a multi-AUV formation reconstruction obstacle avoidance control method based on artificial potential field and affine transformation, combine artificial potential field path planning with affine transformation formation reconstruction method, study the affine transformation multi-AUV formation reconstruction obstacle avoidance control method based on artificial potential field, and solve the multi-AUV formation planning and reconstruction obstacle avoidance problems in complex environments.

[0005] In order to achieve the above purpose, the technical solutions adopted are as follows:

[0006] A multi-AUV formation reconstruction obstacle avoidance control method based on artificial potential field and affine transformation, the method comprising:

[0007] The potential energy function of the improved artificial potential field is established; among them, the gravitational potential energy function of the target area to the virtual AUV is expressed as:

[0008]

[0009] Where U att represents the attractive potential field, k att represents the gravitational coefficient of the gravitational field in the target area, ρ(p v ,p g ) represents the distance between the virtual point and the target area, p v represents a virtual point, p g Indicates the location information of the target point, d g represents the boundary value of the improved attractive potential field;

[0010] The gravitational function is expressed as:

[0011]

[0012] In the formula, F att (p v ) represents the virtual navigator p v The attraction received from the target point, represents the gradient, U att (p v ) represents the virtual navigator pv The attractive potential field of the target point;

[0013] The repulsive potential energy function of the i-th obstacle on the virtual AUV is expressed as:

[0014]

[0015] In the formula, Indicates the proposed leader p v The attraction and repulsion potential field of the obstacle is: the superscript i represents the i-th obstacle, k rep represents the coefficient of the repulsive potential energy function, represents the location information of the ith obstacle, ρ0 represents the influence range radius of the repulsive potential field, m represents the adaptive factor, m>0;

[0016] The repulsive potential energy function is used as the repulsive force acting on the virtual AUV by the obstacle, which is expressed as:

[0017]

[0018] In the formula, represents the repulsive force exerted by the ith obstacle on the virtual AUV, and represent the first component and the second component respectively; p obs Indicates the location information of the obstacle;

[0019] The first component and the second component are expressed as:

[0020]

[0021] In the formula, Point the AUV away from obstacles, Pointing from the AUV to the target point.

[0022] The resultant force on the AUV in the resultant potential field is determined based on the gravitational function and the repulsive potential energy function, which is expressed as:

[0023]

[0024] In the formula, F total The resultant force for improving the position field;

[0025] The repulsive force exerted by the obstacle on the AUV decreases as the relative distance between the AUV and the target decreases, and the degree of reduction is determined by the adaptive factor m.

[0026] Furthermore, the repulsive force exerted by the obstacle on the AUV decreases as the relative distance between the AUV and the target decreases, and the degree of reduction is determined according to the adaptive factor m, including:

[0027] When \(0 \lt m \lt 1\), the AUV successfully reaches the target point. When \(\rho(p v ,p g ) \to 0\), the first component \(F rep1 (p v ) \to 0\), and the second component \(F rep2 (p v ) \to \infty\). The resultant force on the AUV is greater than zero, and the direction of the AUV is from the AUV towards the target point;

[0028] When \(m = 1\) and \(\rho(p v ,p g ) \to 0\), the first component \(F rep1 (p v ) \to 0\), and the second component \(F rep2 (p v ) \to c\), where \(c\) is a constant. The resultant force on the AUV is greater than zero, and the direction of the AUV is from the AUV towards the target point;

[0029] When \(m \gt 1\) and \(F rep1 (p v ) \to 0\), \(F rep (p v ) \to 0\) gradually decreases. Due to the existence of the attractive potential field, the AUV successfully reaches the target point under the generated potential field.

[0030] Furthermore, the method further includes:

[0031] When it is determined that the positive direction of the \(x\)-axis is counterclockwise, at the \(j\)th path point, calculate the angles between the line connecting the AUV and the target point and the three axes, and the angles between the line connecting the AUV and the obstacle and the three axes through the following formulas:

[0032]

[0033] In the formula, \(\alpha gi represents the angle between the attractive force of the target point on the AUV and the \(x\)-axis at the \(j\)th path point, \(\beta gi represents the angle between the attractive force of the target point on the AUV and the \(y\)-axis at the \(j\)th path point, \(\gamma gi represents the angle between the attractive force of the target point on the AUV and the \(z\)-axis at the \(j\)th path point, \(p vj represents the position of the AUV at the \(j\)th path point, \(x g represents the \(x\)-axis coordinate of the target point position, \(x vj represents the \(x\)-axis coordinate of the position of the AUV at the \(j\)th path point, \(y g represents the \(y\)-axis coordinate of the target point position, \(y vj represents the \(y\)-axis coordinate of the position of the AUV at the \(j\)th path point, \(z g represents the \(z\)-axis coordinate of the target point position, \(z vjrepresents the z-axis coordinate of the position of the AUV at the jth path point, α oj represents the angle between the obstacle repulsion force on the AUV at the jth path point and the x-axis, β oj represents the angle between the obstacle repulsion force on the AUV at the jth path point and the y-axis, γ oj represents the angle between the obstacle repulsion force on the AUV at the jth path point and the z-axis, p obs Indicates the location of the obstacle, x obs Indicates the x-axis coordinate of the obstacle's location, y obs Indicates the y-axis coordinate of the obstacle's location, z obs Indicates the x-axis coordinate of the obstacle's location. ;

[0034] Based on the calculated attraction and repulsion, the attraction and repulsion components on the three axes of the AUV at the current path point are calculated using the following formula:

[0035] (F attx (p v ),F atty (p v ),F attz (p v ))=F att (p v )(α gi ,β gi ,γ gi )

[0036] (F rep1x (p v ),F rep1y (p v ),F rep1z (p v ))=F rep1 (p v )(α oj ,β oj ,γ oj )

[0037] (F rep2x (p v ),F rep2y (p v ),F rep2z (p v ))=F rep2 (p v )(α gi ,β gi ,γ gi )

[0038] In the formula, (F attx (p v ),F atty(p v ),F attz (p v )) represents the component forces of the attraction of the target point on the AUV in the directions of the three coordinate axes, F att (p v ) represents the attraction of the target point to the AUV, (F rep1x (p v ),F rep1y (p v ),F rep1z (p v )) represents the component forces of the obstacle's repulsive force 1 on the AUV in the directions of the three coordinate axes, F rep1 (p v ) represents the repulsive force of the obstacle on the AUV, (F rep2x (p v ),F rep2y (p v ),F rep2z (p v )) represents the component forces of the target point's repulsive force 2 on the AUV in the directions of the three coordinate axes, F rep2 (p v ) represents the repulsive force of the target point on the AUV2;

[0039] Calculate the position p of the next path point based on the resultant force j+1 (x j+1 ,y j+1 ,z j+1 ), expressed as:

[0040]

[0041] In the formula, x v(j+1) ,y v(j+1) and z v(j+1) Respectively, α totj , β totj and γ totj They represent the angles between the resultant force and the x, y, and z axes, respectively.

[0042] Furthermore, the method further comprises:

[0043] Constructing a deformation factor; wherein the deformation factor is a parameter for changing the size and shape of the formation structure to pass through the obstacle area while maintaining the topological structure of the nominal configuration of the formation of multiple AUVs during the obstacle avoidance process;

[0044] The switching instruction is determined according to the deformation factor; wherein the switching instruction represents different formation switching modes.

[0045] Furthermore, the deformation factor is expressed as:

[0046]

[0047] Where χ represents the deformation factor, χ>χ m ,x m is the threshold of the formation reconstruction factor, which refers to the minimum value of the deformation factor that ensures that there is no collision between the AUVs in the formation and the obstacles; D is the maximum width of the nominal configuration of the multi-AUV formation, D min It is the minimum width of the obstacle zone that can be passed in the direction of the formation system's advance.

[0048] Furthermore, the switching instruction is represented by σ, σ∈{σ i |i=0,1,2,3}, determine the switching instruction according to the deformation factor, including:

[0049] If χ≥1, D min ≥D, then σ=0, which means that the multi-AUV formation structure maintains the zero transformation mode, indicating that the multi-AUV formation can directly pass through the obstacle area without changing the current formation or rotating the current formation;

[0050] If x min <χ<1, then σ=1, which represents the isomorphic transformation mode of the multi-AUV formation structure, indicating that the formation system cannot directly pass through the obstacle area with the current formation, and can pass through the obstacle area without collision by reducing the formation proportionally or performing shear or displacement deformation;

[0051] If χ<χ m , then σ = 2, which represents the heterogeneous transformation mode of the multi-AUV formation structure, indicating that the formation system can only reconstruct and transform the current formation through the comprehensive operation of shearing and displacement to pass through the obstacle area;

[0052] In three-dimensional space, when the formation AUVs sail to the obstacle waters, σ = 3, which represents the planarization mode of the multi-AUV formation structure. Through the hybrid formation reconstruction method, the formation configuration is transformed to the same plane to pass through the obstacle waters.

[0053] Furthermore, the method further includes constructing a multi-AUV three-dimensional formation reconstruction obstacle avoidance controller; wherein constructing a multi-AUV three-dimensional formation reconstruction obstacle avoidance controller specifically includes:

[0054] Using p i =(x i ,y i ,z i ) indicates AUV i Position in three-dimensional space, where i = (1, 2.....n), v i =(v xi ,v yi ,v zi ) represents the velocity vector of the i-th AUV, u i =(uxi ,u yi ,u zi ) represents the control input of the i-th AUV, then:

[0055]

[0056] In the formula, Indicates the AUVi position p i The first derivative of , i.e., velocity, represents AUVi speed v i The first derivative of is acceleration;

[0057] The relative position vector between any two AUVs is determined as follows:

[0058]

[0059] In the formula, represents the relative position between AUVi position and AUVj position, p i represents the AUVi position, p j represents the position of AUVj;

[0060] Distance error i It is expressed as:

[0061]

[0062] Where, d ij represents the expected formation distance between AUVi and AUVj in the expected formation;

[0063] The distance error dynamics is determined as:

[0064]

[0065] In the formula, Indicates the distance error e ij The first derivative of , t represents time, Denotes the expected distance d ij The first-order derivative of , T represents the transpose of the matrix;

[0066] Introduce the following alternative error variable z ij :

[0067]

[0068] The distance error is used to verify the alternative error variable, which is expressed as:

[0069]

[0070] Calculate z by the following formula ijThe ring opening dynamics are:

[0071]

[0072] In the formula, Represents the parameter variable z ij The first derivative of ;

[0073] Introduce the Lyapunov function, expressed as

[0074]

[0075] Where W ij represents the introduced Lyapunov function, and z represents the introduced parameter variable;

[0076] Define the first function as follows:

[0077]

[0078] Where W(e) represents the introduced Lyapunov function, E* represents the edge set of the formation, and e ij Indicates distance error;

[0079] The time derivative of the first function along the range error dynamics is given by:

[0080]

[0081] In the formula, represents the first-order derivative of W(e);

[0082] Given the stiffness matrix R(p):

[0083]

[0084] where f(p)=(...,||p i -p j || 2 ,...), represents the distance matrix of multiple AUVs, p=(p1,p2,...,p n ), represents the position of each AUV in the formation.

[0085] The time derivative of the first function along the range error dynamics is written as:

[0086]

[0087] in represents the speed of each AUV in the formation, Represents a variable related to distance.

[0088] Introduce the following variable s:

[0089] s=vv d

[0090] In the formula, Indicates virtual speed input; represents a natural number set, n represents the number of AUVs in the formation;

[0091] Introduce the second function:

[0092]

[0093] Where W d (e, s) represents the introduced Lyapunov function;

[0094] After taking the derivative of the second function, we get:

[0095]

[0096] in is the control input for each AUV in the formation, W d The first derivative of (e,s), represents the expected speed v d The first derivative of ;

[0097] Determine the control law:

[0098]

[0099] u a =R + (p)(-k v z+d v )

[0100] where k a ,k v >0,k a ,k v Respectively represent the constant coefficient; R + (p) denotes the Moore-Penrose pseudoinverse of R(p), v m Indicates the speed of the fleet, v d represents the expected movement speed of the formation, u a represents the formation formation control input;

[0101] The obstacle avoidance controller for reconstructing the multi-AUV three-dimensional formation is determined as:

[0102] u=-k a s+h d -R T (p)z

[0103]

[0104] u a =R + (p)(-k v z+d v )

[0105] In the formula, k t is a positive constant, e t =p v -p1 is the position error between the virtual pilot AUV and AUV1, V v is the navigation speed of the virtual AUV, i.e., the planning path step length, k s is a positive constant and h d express, Indicates v d The first derivative of Indicates u a The first derivative of .

[0106] The beneficial effects of the present invention are:

[0107] The present invention improves the artificial potential field to effectively solve the problem of unreachable target area in the traditional artificial potential field method. For the first time, the powerful deformation ability of affine transformation is effectively combined with the efficient obstacle avoidance efficiency of IAPF, and the obstacle avoidance methods such as formation contraction, formation displacement, formation shearing and formation transfer based on affine transformation are proposed, providing formation transformation options for the overall obstacle avoidance of multiple AUV formations. BRIEF DESCRIPTION OF THE DRAWINGS

[0108] Figure 1 The obstacle avoidance motion trajectory of multiple AUV formations based on affine scaling according to an embodiment of the present invention is shown. Figure 1 .

[0109] Figure 2 The obstacle avoidance motion trajectory of multiple AUV formations based on affine scaling according to an embodiment of the present invention is shown. Figure 2 .

[0110] Figure 3 The obstacle avoidance distance error and control input of a multi-AUV formation based on affine scaling according to an embodiment of the present invention are shown. Figure 1 .

[0111] Figure 4 The obstacle avoidance distance error and control input of a multi-AUV formation based on affine scaling according to an embodiment of the present invention are shown. Figure 2 .

[0112] Figure 5 The obstacle avoidance motion trajectory of multiple AUVs based on formation displacement according to an embodiment of the present invention is shown. Figure 1 ;

[0113] Figure 6 The obstacle avoidance motion trajectory of multiple AUVs based on formation displacement according to an embodiment of the present invention is shown. Figure 2 ;

[0114] Figure 7 The obstacle avoidance distance error and control input of a multi-AUV formation based on affine displacement according to an embodiment of the present invention are shown. Figure 1 ;

[0115] Figure 8 The obstacle avoidance distance error and control input of a multi-AUV formation based on affine displacement according to an embodiment of the present invention are shown. Figure 2 ;

[0116] Fig. 9 The obstacle avoidance motion trajectory of multiple AUVs based on formation shearing according to an embodiment of the present invention is shown. Figure 1 ;

[0117] Fig.10 The obstacle avoidance motion trajectory of multiple AUVs based on formation shearing according to an embodiment of the present invention is shown. Figure 2 ;

[0118] Fig.11 The obstacle avoidance distance error and control input of a multi-AUV formation based on affine shearing according to an embodiment of the present invention are shown. Figure 1 ;

[0119] Fig.12 The obstacle avoidance distance error and control input of a multi-AUV formation based on affine shearing according to an embodiment of the present invention are shown. Figure 2 ;

[0120] Fig.13 The navigation and obstacle avoidance trajectory of multiple AUV formations in a multi-dynamic obstacle (two-directional motion) environment according to an embodiment of the present invention is shown. Figure 1 ;

[0121] Fig.14 The navigation and obstacle avoidance trajectory of multiple AUV formations in a multi-dynamic obstacle (two-directional motion) environment according to an embodiment of the present invention is shown. Figure 2 ;

[0122] Fig.15 The figure shows the navigation and obstacle avoidance trajectory of a multi-AUV formation in a multi-dynamic obstacle (a directional motion and a random motion) environment according to an embodiment of the present invention. Figure 1 ;

[0123] Fig.16 The figure shows the navigation and obstacle avoidance trajectory of a multi-AUV formation in a multi-dynamic obstacle (a directional motion and a random motion) environment according to an embodiment of the present invention. Figure 2 . DETAILED DESCRIPTION

[0124] The following describes the embodiments of the present invention by specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the following embodiments and features in the embodiments can be combined with each other without conflict.

[0125] The specific implementation of the present invention is further described in detail below in conjunction with the drawings and examples.

[0126] In the current existing technology, when performing deformation monitoring, a single sensor layout is usually used to achieve monitoring, such as only using a total station, a laser rangefinder or a strain gauge. This single sensor layout method leads to insufficient accuracy in stress and deformation monitoring of the entire section, and there is room for further improvement.

[0127] The embodiment of the present invention provides a multi-AUV formation reconstruction obstacle avoidance control method based on artificial potential field and affine transformation, the method includes the construction of an improved artificial potential field function and the design of a multi-AUV formation reconstruction obstacle avoidance controller based on affine transformation. The construction of the improved artificial potential field function specifically includes:

[0128] The potential energy function of the improved artificial potential field is designed, in which the gravitational potential energy function of the target area to the virtual AUV is:

[0129]

[0130] Then the gravitational function is:

[0131]

[0132] Among them, k att represents the gravitational coefficient of the gravitational field in the target area; ρ(p v ,p g ) represents the virtual point p v The distance paradigm to the target area, ρ(p v ,p g )=||p v -p g ||, d g To improve the boundary value of the attractive potential field, when the distance between the virtual AUV and the target is greater than d g From (1) and (2), we can see that the growth rate of the attractive potential field slows down, and the attractive force remains unchanged. g The location information of the target point.

[0133] For the improved adaptive repulsive potential field, the repulsive potential function of the $i$-th obstacle acting on the virtual AUV is selected

[0134]

[0135] where the superscript $i$ represents the $i$-th obstacle, and $k$ rep is the coefficient of the repulsive potential energy function, $\mathbf{r}_i$ is the position information of the $i$-th obstacle, $\rho_0$ is the influence range radius of the repulsive potential field, and $m$ is the adaptive factor with $m > 0$;

[0136] Then the repulsive force exerted by the obstacle on the virtual AUV is the negative gradient of the repulsive potential energy function:

[0137]

[0138] When the AUV is within the influence range of the repulsive force, the corresponding improved repulsive force consists of the following two components:

[0139]

[0140] points away from the obstacle AUV, points from the AUV to the target point.

[0141] The resultant force on the AUV in the combined potential field is:

[0142]

[0143] $\mathbf{F}$ total is the resultant force for potential field improvement.

[0144] The improved repulsive function decreases as the relative distance $\rho(\mathbf{p}$ v , $\mathbf{p}$ g ) between the AUV and the target decreases, and the degree of decrease is related to the value of the adaptive factor $m$.

[0145] When $0 < m < 1$, the AUV reaches the target point smoothly. That is to say, when $\rho(\mathbf{p}$ v , $\mathbf{p}$ g ) $\to 0$, the first component $\mathbf{F}$ rep1 $(\mathbf{p}$ v ) $\to 0$, and the second component $\mathbf{F}$ rep2 $(\mathbf{p}$ v ) $\to \infty$. The resultant force on the AUV is greater than zero, and the direction of the AUV is from the AUV to the target. For this reason, the AUV can reach the target point smoothly.

[0146] When $m = 1$ and $\rho(\mathbf{p}$ v , $\mathbf{p}$ g ) $\to 0$, the first vector component $\mathbf{F}$ rep1 $(\mathbf{p}$ v)→0, the second vector component F rep2 (p v )→c (c is a constant), the resultant force on the AUV is greater than zero, and the direction of the AUV is from the AUV to the target. Because of this, the AUV can reach the target point smoothly.

[0147] When m>1 and F rep1 (p v )→0, the improved repulsion function F rep (p v )→0 also gradually decreases. Finally, F rep (p v )→0. Due to the existence of the attractive potential field, the AUV can reach the target smoothly under the generated potential field.

[0148] According to the improved adaptive method, the line connecting the AUV and the target point and the angles between the three axes can be calculated separately at the jth path point. The line connecting the AUV and the obstacle and the angles between the three axes can also be calculated separately, and the x-axis is uniformly defined as counterclockwise positive. The calculation process is as follows:

[0149]

[0150] According to the attraction and repulsion obtained by the improved adaptive artificial potential field method, the attraction and repulsion components on the three axes of the AUV at the current path point are calculated. The calculation process is as follows:

[0151] (F attx (p v ),F atty (p v ),F attz (p v ))=F att (p v )(α gi ,β gi ,γ gi ) (10)

[0152] (F rep1x (p v ),F rep1y (p v ),F rep1z (p v ))=F rep1 (p v )(α oj ,β oj ,γ oj ) (11)

[0153] (F rep2x (p v ),F rep2y (p v),F rep2z (p v ))=F rep2 (p v )(α gi ,β gi ,γ gi ) (12) Calculate the position p of the next path point based on the resultant force j+1 (x j+1 ,y j+1 ,z j+1 )for:

[0154]

[0155] where α totj , β totj , γ totj is the angle between the resultant force and the x, y, and z axes.

[0156] The obstacle avoidance algorithm of the formation AUV system is mainly composed of formula (2), that is, the gravitational force F of the target point on the virtual AUV att , Formula (4) is the repulsive force F of the ith obstacle on the virtual AUV rep ,Equation (7) is the resultant force of all repulsive and attractive forces on the virtual AUV in the potential field.,When performing tasks or navigating, a multi-AUV formation needs to adapt to the constraints of obstacles in the surrounding environment and perform effective formation reconstruction and obstacle avoidance in real time.

[0157] A single AUV uses the artificial potential field method to quickly avoid obstacles and reach the target point. To achieve obstacle avoidance for multiple AUVs in the process of advancing in formation, it is necessary to comprehensively consider the formation transformation strategy. The deformation factor refers to the parameters of the formation formation of multiple AUVs that maintain the topological structure of the nominal configuration unchanged during the obstacle avoidance process (that is, the topological relationship between each AUV remains unchanged), and changes the size and shape of the formation structure to pass through the obstacle area by using the affine transformation method.

[0158] At the same time, the size of the deformation factor affects the switching of the formation mode. The formation modes include the following five types: formation zero transformation, formation scaling transformation, formation shear transformation, formation displacement transformation and formation rotation transformation. The expression of the deformation factor is as follows:

[0159]

[0160] Where χ represents the deformation factor, χ>χ m ,x m is the threshold of the formation reconstruction factor, which refers to the minimum value of the deformation factor that ensures that there is no collision between the AUVs in the formation and the obstacles. D is the maximum width of the nominal configuration of the multi-AUV formation, D minis the minimum width of the traversable obstacle zone in the advancing direction of the formation system. When multiple AUVs advance in formation, the leading AUV calculates the current deformation factor χ according to the width of the traversable obstacle zone, and then determines the switching instruction σ of the formation mode according to the value of χ, σ∈{σ i |i=0,1,2,3}. The instruction σ represents three formation switching modes:

[0161] (1) The multi-AUV formation structure maintains zero transformation mode (σ = 0)

[0162] If χ≥1, then D min ≥D, indicating that the multi-AUV formation can directly pass through the obstacle area without changing the current formation or rotating the current formation appropriately. This mode is the optimal strategy for multi-AUV formation to avoid obstacles.

[0163] Multi-AUV formation structure isomorphic transformation mode (σ=1)

[0164] If x min <χ<1, indicating that the formation system cannot directly pass through the obstacle area with the current formation, but can scale down the formation without changing the formation shape, or perform shearing or displacement deformation as needed, and then pass through the obstacle area without collision. Among them, the scaling mode does not change the formation shape and is easy to restore the original formation, which is the suboptimal strategy for formation switching and obstacle avoidance.

[0165] Heterogeneous transformation mode of multi-AUV formation structure (σ=2)

[0166] When the navigation conditions of the formation keeping mode and the isomorphic transformation mode are not met, that is, χ<χ m , indicating that the formation system can only reconstruct and transform the current formation through comprehensive operations such as shearing and displacement. Only then can it pass through the obstacle area. This is a more complex transformation method.

[0167] Planarization mode of multi-AUV formation structure (σ=3)

[0168] In three-dimensional space, when the formation AUVs sail to waters with very harsh obstacle environment, the formation cannot pass through the obstacle area in a three-dimensional structure and can only sail in a plane. The formation configuration can be transformed to the same plane through mixed formation reconstruction, and then pass through the obstacle waters. This is a more complex transformation method.

[0169] Through the dynamic formation transformation obstacle avoidance strategy, when multiple AUVs are advancing in formation, the leading AUV can select the appropriate formation switching mode according to the situation in the obstacle area, and then reconstruct the formation structure according to the affine transformation method to avoid obstacles, and then restore the formation to maintain the original formation and improve the overall flexibility of the formation.

[0170] Use p i =(x i ,yi ,z i ) indicates AUV i Position in three-dimensional space, where i = (1, 2.....n), v i =(v xi ,v yi ,v zi ) represents the velocity vector of the i-th AUV, u i =(u xi ,u yi ,u zi ) represents the control input of the i-th AUV, then:

[0171]

[0172] The relative position vector between any two AUVs is defined as follows:

[0173]

[0174] The distance error is:

[0175]

[0176] From (15) and (17), we know that the distance error dynamics is:

[0177]

[0178] In order to simplify the subsequent control design and facilitate stability analysis, the following alternative error variables are introduced

[0179]

[0180] Using (18), we can verify that (20) can be expressed as

[0181]

[0182] Using (15)(16)(20)(21) we can calculate z ij The ring opening dynamics are:

[0183]

[0184] Now, consider the Lyapunov function:

[0185]

[0186] where z=[...,z ij ,...].

[0187] From (20), we know that (23) in e ijis positive definite and radially unbounded. Now define the following function:

[0188]

[0189] where e=(...,e ij ,...),(24)The time derivative along (19) is given by:

[0190]

[0191] This gives the rigidity matrix:

[0192]

[0193] where f(p)=(...,||p i -p j || 2 ,...) is the distance matrix of multiple AUVs, p=(p1,p2,...,p n ).

[0194] Combining (25) and (20), we know that (24) can be written as

[0195]

[0196] in Following the backstepping control technique, the variables are introduced:

[0197] s=v-vd (28)

[0198] in Indicates virtual speed input.

[0199] Then introduce the function:

[0200]

[0201] After taking the derivative of (29), we get:

[0202]

[0203] in It is the control input for each AUV in the formation.

[0204] The following theorem states the control law:

[0205]

[0206] u a =R + (p)(-k v z+d v ) (33)

[0207] where k a ,k v >0,R + (p) is the Moore-Penrose pseudoinverse of R(p), v m is the formation sailing speed.

[0208] Obstacle avoidance control and reconstruction of multiple AUVs in three-dimensional formation based on improved artificial potential field and affine transformation algorithm

[0209] The device is:

[0210] u=-k a s+h d -R T (p)z(34a)

[0211]

[0212] u a =R + (p)(-k v z+d v )(34d)

[0213] Where s is defined in (28), k t is a positive constant, e t =p v -p1 is the position error between the virtual pilot AUV and AUV1, V v is the navigation speed of the virtual AUV, i.e. the planning path step length, K s is a positive constant and The definitions of other parameters are the same as (31)(32)(33).

[0214] like Figure 1 As shown, the flowchart fully and in detail illustrates the complete obstacle avoidance process of the formation from its start to its arrival at the destination.

[0215] In order to verify the effectiveness of the designed multi-AUV three-dimensional formation and reconstruction obstacle avoidance method, multiple groups of simulation experiments were carried out in Matlab software based on the improved artificial potential field method (IAPF).

[0216] Design a three-layer formation control strategy, the first layer is a virtual pilot AUVp v ,Using the aforementioned obstacle avoidance path planning method, dynamic path planning is performed on the virtual pilot AUV in a three-dimensional environment.

[0217] The second layer is the expected formation configuration. According to the position and environmental conditions of the virtual pilot AUV, the formation structure is determined using the affine transformation method.

[0218] The third layer designs the following AUV navigation tracking controller through the aforementioned path tracking method, so that the leading AUV tracks the virtual navigator, and the following AUV keeps tracking the expected formation to keep the formation structure unchanged or in a time-varying state.

[0219] To verify the universality of the obstacle avoidance maneuvering strategy of multiple AUVs based on IAPF and affine transformation algorithm proposed in this invention. Through different decisions for different obstacle scenarios, the obstacle avoidance function in different obstacle scenarios is verified. Consider a formation system composed of 9 AUVs, where AUV9 is the leader and the other AUVs are followers. Select the target point to virtual AUV gravitational field gain coefficient k att = 10 Repulsion gain coefficient k between obstacle and virtual AUV rep =20, the maximum obstacle impact distance d max =10, ρ0=1, artificially set and generate (can also be randomly generated) the positions of multiple obstacles in three-dimensional space. The initial coordinates of the virtual AUV are p v (0) = (1,1,1), the target point coordinates are p g =(40,35,30), the motion trajectory simulation results of 9 AUVs in three-dimensional space are as follows Figures 3 to 16 shown.

[0220] like Figures 1 to 4 As shown, Figure 1 and Figure 2 This is a schematic diagram of the obstacle avoidance motion trajectory of multiple AUV formations based on affine scaling. Figure 2 and Figure 3 Schematic diagram of the corresponding distance error and control input.

[0221] The running trajectories of 9 AUVs based on affine scaling formation in a three-dimensional multi-obstacle space are shown in the figure below. Figure 1 and Figure 2 As shown in the figure, there are 8 static spherical obstacles and 2 static cylindrical obstacles. The trajectory of the virtual pilot AUV (planned path point) is a "cross" gray solid line, and the trajectories of the pilot AUV and the follower AUV are black thick solid line and gray thin solid line respectively. The pilot AUV sails along the path of the virtual AUV and can quickly track the virtual AUV trajectory. Other follower AUVs can track the pilot AUV and maintain the desired relative distance to form a stable cube formation. Figure 1 It is a 3D view of multi-AUV formation affine zoom obstacle avoidance. Figure 2 This is the corresponding top view. Combining the two figures, we can more clearly see the changes in the formation structure of multiple AUVs during formation navigation and obstacle avoidance. That is, at the beginning, they sail in a formation of a certain size. When the AUV detects an obstacle in front and judges the distance between the obstacles, it chooses to reduce the formation structure to avoid the obstacle and successfully navigates. This proves the feasibility of the multi-AUV system based on formation scaling and reconstruction to avoid obstacles.

[0222] Figure 3 The error between the distance between any two AUVs in a multi-AUV formation and the expected formation distance is shown. It can be seen that the distance error can quickly converge to zero in a short time. The formation has higher control accuracy and faster convergence speed, as well as good transient performance. The control performance of the formation is significantly improved. Figure 4 Represents the control input curve of each AUV in the formation in the directions of the three coordinate axes x, y, and z.

[0223] like Figures 5 to 8 As shown, Figure 5 and Figure 6 is the motion trajectory diagram of multiple AUVs avoiding obstacles based on formation displacement, Figure 7 and Figure 8 The obstacle avoidance distance error and control input diagram of multiple AUV formation based on affine displacement.

[0224] Figures 5 to 8 To carry out the simulation results of multi-AUV formation displacement obstacle avoidance, at the initial moment, the nominal cube configuration is still formed. The static obstacles are modeled as 7 spheres, a cube, 2 cylinders and a cone. Figure 5 and Figure 6 It represents the displacement obstacle avoidance trajectory diagram of multiple AUV formations. It can be seen that under the action of the formation controller, the follower AUV and the lead AUV maintain the expected relative distance to form a cube formation. When the formation sails for a distance, the detection sonar detects that there is an obstacle blocking the running track ahead. After calculation, it is confirmed that the passable safe distance width is less than the width of the AUV formation. The formation needs to be transformed to pass through the obstacle area. The formation transformation decision mechanism designed in the previous article makes the formation transformation decision. At this time, it only needs to compress the current formation width to pass through the obstacle area smoothly. Under the control of the affine transformation controller, the AUV formation performs formation displacement reconstruction and deformation, and then passes through the obstacle area without colliding with the obstacle.

[0225] from Figure 7 and Figure 8 It can be seen that the navigation trajectories of each AUV do not touch external obstacles, indicating that there is no collision, and the obstacle avoidance effect of no collision between the formation reconstruction navigation and obstacles is achieved. At the same time, although the navigation trajectories of each AUV overlap in some areas, from the shape of the formation structure at the current moment represented by the gray and white lines in the figure, it can be found that the positions of the 9 AUVs at the same time meet the set values ​​of the formation reconstruction shape, the spacing is always above the minimum collision safety distance, and the overlap of trajectories at the same position in space does not occur at the same time. Therefore, the formation configuration during navigation meets the constraints of the minimum safety distance between AUVs, and no collision between AUVs occurs, which is conducive to the safe navigation of AUVs. Figure 7It represents the error between the distance between any two AUVs in a multi-AUV formation and the expected formation distance. It can be seen that when the formation changes, the relative distance error of the formation is in a changing state. The amplitude of this change is related to the displacement distance of the AUVs that have been displaced in the formation, but this value is basically in a stable changing state without oscillation. Figure 8 It represents the control input curve of each AUV in the formation in the directions of the three coordinate axes: x, y, and z.

[0226] like Figures 9 to 12 As shown, Fig. 9 and Fig.10 It is the motion trajectory diagram of multiple AUVs avoiding obstacles based on formation cutting; Fig.11 and Fig.12 It is the obstacle avoidance distance error and control input diagram of multiple AUV formation based on affine shearing;

[0227] Figures 9 to 12 This is the result of the multi-AUV formation shear obstacle avoidance simulation. At the initial moment, it still forms a nominal cube configuration. Static obstacles are modeled as 2 spheres and 2 cuboids to simulate larger or dense obstacles. The trajectory of the 9 AUV formation shear obstacle avoidance is shown in the figure below. Fig. 9 and Fig.10 shown. Fig.10 This is the xoz plane view. It can be seen from the figure that when the obstacle space is relatively narrow, the AUV formation is basically transformed to the same plane through shear transformation.

[0228] Fig.11 It represents the error between the distance between any two AUVs in a multi-AUV formation and the expected formation distance. Fig.12 It represents the control input curve of each AUV in the formation in the direction of the three coordinate axes x, y, and z. It can be seen that the distance error of the AUV formation is in a dynamic stable time-varying state with the transformation of the formation, and smoothly converges to the zero domain or near a specific value.

[0229] like Figures 13 to 16 As shown, Fig.13 and Fig.14 It is the navigation and obstacle avoidance trajectory diagram of multiple AUV formations in a multi-dynamic obstacle (two-directional motion) environment; Fig.15 and Fig.16 It is the navigation and obstacle avoidance trajectory of multiple AUV formations in a multi-dynamic obstacle (directional motion and random motion) environment;

[0230] Figures 13 to 16 The following simulation results are given:

[0231] In order to verify the obstacle avoidance effect of the formation reconstruction obstacle avoidance method proposed in the previous article in the three-dimensional dynamic obstacle space, dynamic obstacles are set in the formation navigation space for simulation verification. To increase the complexity of the environment, the following two operating environments are set.

[0232] Scenario 1: Set up 8 static spherical obstacles, 2 static cylindrical obstacles, and two directional uniform motion obstacles (starting positions and directions are p do1 (0) = (20, 45, 40), direction (-1, 1, 1), p do2 (0) = (20, 5, 5), direction (0.5, -1, -1)).

[0233] Scenario 2: Set up 8 static spherical obstacles, 2 static cylindrical obstacles, 1 directional uniform motion obstacle and 1 random direction motion obstacle (starting position and direction are p do1 (0) = (5, 10, 10), random direction, p do2 (0) = (40,5,5), direction (-1,1,1).

[0234] Similar to the static obstacle environment experiment, the thick black solid line represents the trajectory of the virtual AUV (the obstacle avoidance path planned by IAPF), and the thin gray solid lines represent the navigation trajectories of each follower AUV.

[0235] Fig.13 and Fig.14 Obstacle avoidance effect of multiple AUVs in multiple dynamic obstacle scenario 1. Fig.15 and Fig.16 The obstacle avoidance effect of multiple AUVs in multiple dynamic obstacle scenario 2. By comparison, it can be found that the randomly moving obstacles have a greater impact on the formation's obstacle avoidance path planning, but the formation can still reach the destination in the end. The simulation results show that when the formation reconstruction obstacle avoidance maneuver strategy is applied to the environment with multiple dynamic obstacles using the decision unit of the entire mission system, the AUV can also complete the obstacle avoidance task quickly and well.

[0236] In summary, the overall obstacle avoidance strategy based on IAPF and affine transformation proposed in the present invention has good obstacle avoidance capability in multi-AUV formation, and the AUV formation can make the optimal formation change selection according to different obstacle environments. After passing through the obstacle area, the formation can be restored to the original formation state, and there will be no large error fluctuations when the formation is changed. They can quickly converge to their respective steady-state values, and the integrity of the multi-AUV formation and the stability during the obstacle avoidance process are effectively improved.

[0237] The above implementation modes are only used to illustrate the present invention, but not to limit the present invention. Ordinary technicians in the relevant technical field can make various changes and modifications without departing from the spirit and scope of the present invention. Therefore, all equivalent technical solutions also belong to the scope of the present invention. The patent protection scope of the present invention should be defined by the claims.

Claims

1. A multi-AUV formation reconstruction obstacle avoidance control method based on artificial potential field and affine transformation, characterized in that: include: The potential energy function of the improved artificial potential field is established; among them, the gravitational potential energy function of the target area to the virtual AUV is expressed as: Where U att represents the attractive potential field, k att represents the gravitational coefficient of the gravitational field in the target area, ρ(p v ,p g ) represents the distance between the virtual point and the target area, p v represents a virtual point, p g Indicates the location information of the target point, d g represents the boundary value of the improved attractive potential field; The gravitational function is expressed as: In the formula, F att (p v ) indicates p v The attraction of the point to the target point, ▽ represents the gradient, U att (p v ) indicates p v The attractive potential field of the target point on the point; The repulsive potential energy function of the i-th obstacle on the virtual AUV is expressed as: In the formula, Indicates p v The repulsive force of the obstacle at point i, the superscript i represents the i-th obstacle, k rep represents the coefficient of the repulsive potential energy function, represents the location information of the ith obstacle, ρ0 represents the influence range radius of the repulsive potential field, m represents the adaptive factor, m>0; The repulsive potential energy function is used as the repulsive force acting on the virtual AUV by the obstacle, which is expressed as: In the formula, represents the repulsive force exerted by the ith obstacle on the virtual AUV, and represent the first component and the second component respectively; p obs Indicates the location information of the obstacle; The first component and the second component are expressed as: In the formula, Point the AUV away from obstacles, Pointing from the AUV to the target point. The resultant force on the AUV in the resultant potential field is determined based on the gravitational function and the repulsive potential energy function, which is expressed as: In the formula, F total The resultant force for improving the position field; The repulsive force exerted by the obstacle on the AUV decreases as the relative distance between the AUV and the target decreases, and the degree of reduction is determined by the adaptive factor m.

2. The multi-AUV formation reconstruction obstacle avoidance control method based on artificial potential field and affine transformation as claimed in claim 1, characterized in that: The repulsive force exerted by the obstacle on the AUV decreases as the relative distance between the AUV and the target decreases. The degree of reduction is determined by the adaptive factor m, including: When 0 < m < 1, the AUV successfully reaches the target point. When ρ(p v , p g ) approaches 0, the first component F rep1 (p v ) approaches 0, and the second component F rep2 (p v ) approaches ∞. The resultant force acting on the AUV is greater than zero, and the direction of the AUV is from the AUV towards the target point. When m = 1, and ρ(p v ,p g )→0, the first component F rep1 (p v )→0, the second component F rep2 (p v )→c, c is a constant, the resultant force on the AUV is greater than zero, and the direction of the AUV is from the AUV to the target point; When m>1, and F rep1 (p v )→0, F rep (p v )→0 gradually decreases. Due to the existence of the attractive potential field, the AUV successfully reaches the target point under the generated potential field.

3. The multi-AUV formation reconstruction obstacle avoidance control method based on artificial potential field and affine transformation as claimed in claim 2, characterized in that: The method further comprises: When the x-axis is determined to be counterclockwise positive, the angle between the line connecting the AUV and the target point and the three axes, as well as the angle between the line connecting the AUV and the obstacle and the three axes are calculated at the jth path point using the following formula: In the formula, α gi represents the angle between the attraction of the target point and the x-axis at the jth path point of the AUV, β gi represents the angle between the attraction of the target point and the y-axis at the jth path point of the AUV, γ gi represents the angle between the attraction of the target point and the z-axis at the jth path point of the AUV, p vj represents the position of the AUV at the jth path point, x g Indicates the x-axis coordinate of the target point. vj represents the x-axis coordinate of the position of the AUV at the jth path point, and y g Indicates the y-axis coordinate of the target point. vj represents the y-axis coordinate of the position of the AUV at the jth path point, z g Indicates the z-axis coordinate of the target point. vj represents the z-axis coordinate of the position of the AUV at the jth path point, α oj represents the angle between the obstacle repulsion force on the AUV at the jth path point and the x-axis, β oj represents the angle between the obstacle repulsion force on the AUV at the jth path point and the y-axis, γ oj represents the angle between the obstacle repulsion force on the AUV at the jth path point and the z-axis, p obs Indicates the location of the obstacle, x obs Indicates the x-axis coordinate of the obstacle's location, y obs Indicates the y-axis coordinate of the obstacle's location, z obs Indicates the x-axis coordinate of the obstacle's location; Based on the calculated attraction and repulsion, the attraction and repulsion components on the three axes of the AUV at the current path point are calculated using the following formula: (F attx (p v ),F atty (p v ),F attz (p v ))=F att (p v )(α gi ,β gi ,γ gi ) (F rep1x (p v ),F rep1y (p v ),F rep1z (p v ))=F rep1 (p v )(α oj ,β oj ,γ oj ) (F rep2x (p v ),F rep2y (p v ),F rep2z (p v ))=F rep2 (p v )(α gi ,β gi ,γ gi ) In the formula, (F attx (p v ),F atty (p v ),F attz (p v )) represents the component forces of the attraction of the target point on the AUV in the directions of the three coordinate axes, F att (p v ) represents the attraction of the target point to the AUV, (F rep1x (p v ),F rep1y (p v ),F rep1z (p v )) represents the component forces of the obstacle's repulsive force 1 on the AUV in the directions of the three coordinate axes, F rep1 (p v ) represents the repulsive force of the obstacle on the AUV, (F rep2x (p v ),F rep2y (p v ),F rep2z (p v )) represents the component forces of the target point's repulsive force 2 on the AUV in the directions of the three coordinate axes, F rep2 (p v ) represents the repulsive force of the target point on the AUV2; Calculate the position p of the next path point based on the resultant force j+1 (x j+1 ,y j+1 ,z j+1 ), expressed as: In the formula, x v(j+1) ,y v(j+1) and z v(j+1) Respectively, α totj , β totj and γ totj They represent the angles between the resultant force and the x, y, and z axes, respectively.

4. The multi-AUV formation reconstruction obstacle avoidance control method based on artificial potential field and affine transformation as claimed in claim 3, characterized in that: The method further comprises: Constructing a deformation factor; wherein the deformation factor is a parameter for changing the size and shape of the formation structure to pass through the obstacle area while maintaining the topological structure of the nominal configuration of the formation of multiple AUVs during the obstacle avoidance process; The switching instruction is determined according to the deformation factor; wherein the switching instruction represents different formation switching modes.

5. The multi-AUV formation reconstruction obstacle avoidance control method based on artificial potential field and affine transformation as claimed in claim 4, characterized in that: The expression of the deformation factor is: Where χ represents the deformation factor, χ>χ m ,x m is the threshold of the formation reconstruction factor, which refers to the minimum value of the deformation factor that ensures that there is no collision between the AUVs in the formation and obstacles; D is the maximum width of the nominal configuration of multiple AUV formations, D min It is the minimum width of the obstacle zone that can be passed in the direction of the formation system's advance.

6. The multi-AUV formation reconstruction obstacle avoidance control method based on artificial potential field and affine transformation as claimed in claim 5, characterized in that: The switching instruction is represented by σ, σ∈{σ i |i=0,1,2,3}, determine the switching instruction according to the deformation factor, including: If χ≥1, D min ≥D, then σ=0, which means that the multi-AUV formation structure maintains the zero transformation mode, indicating that the multi-AUV formation can directly pass through the obstacle area without changing the current formation or rotating the current formation; If x min <χ<1, then σ=1, which represents the isomorphic transformation mode of the multi-AUV formation structure, indicating that the formation system cannot directly pass through the obstacle area with the current formation, and can pass through the obstacle area without collision by reducing the formation proportionally or performing shear or displacement deformation; If χ<χ m , then σ = 2, which represents the heterogeneous transformation mode of the multi-AUV formation structure, indicating that the formation system can only reconstruct and transform the current formation through the comprehensive operation of shearing and displacement to pass through the obstacle area; In three-dimensional space, when the formation AUVs sail to the obstacle waters, σ = 3, which represents the planarization mode of the multi-AUV formation structure. Through the hybrid formation reconstruction method, the formation configuration is transformed to the same plane to pass through the obstacle waters.

7. The multi-AUV formation reconstruction obstacle avoidance control method based on artificial potential field and affine transformation as claimed in claim 4, characterized in that: The method further includes constructing a multi-AUV three-dimensional formation reconstruction obstacle avoidance controller; wherein constructing the multi-AUV three-dimensional formation reconstruction obstacle avoidance controller specifically includes: Using p i =(x i ,y i ,z i ) indicates AUV i Position in three-dimensional space, where i = (1, 2.....n), v i =(v xi ,v yi ,v zi ) represents the velocity vector of the i-th AUV, u i =(u xi ,u yi ,u zi ) represents the control input of the i-th AUV, then: In the formula, represents the first-order derivative of the AUV position, i.e., the velocity of the AUV, represents the first-order derivative of the AUV velocity, i.e., the acceleration input of the AUV; The relative position vector between any two AUVs is determined as follows: In the formula, represents the relative position between any two AUVi and j, p i represents the position of AUVi, p j represents the position of AUVj; Distance error i It is expressed as: Where, d ij represents the expected relative distance between any two AUVi and j in the expected formation; The distance error dynamics is determined as: In the formula, Indicates e ij The first derivative of , t represents time, Indicates d ij The first-order derivative of , T represents the transpose of the matrix; Introduce the following alternative error variable z ij : The distance error is used to verify the alternative error variable, which is expressed as: Calculate z by the following formula ij The ring opening dynamics are: In the formula, Indicates z ij The first derivative of ; Introduce the Lyapunov function, expressed as Where W ij represents the introduced Lyapunov function, and z represents the introduced variable; Define the first function as follows: Where W(e) represents the function designed by Lyapunov, E* represents the desired formation edge set, and e ij Indicates the distance error between the actual distance and the expected distance; The time derivative of the first function along the range error dynamics is given by: In the formula, represents the first derivative of the function W(e); Given the rigidity matrix R(p): where f(p)=(...,||p i -p j || 2 ,...), represents the distance matrix of multiple AUVs, p=(p1,p2,...,p n ), indicating the position information of each AUV in the formation. The time derivative of the first function along the range error dynamics is written as: in Indicates the speed information of each AUV in the formation, Represents a distance variable, . Introduce the following variable s: s=vv d In the formula, Indicates virtual speed input; represents a natural number set, n represents the number of AUVs; Introduce the second function: Where W d (e, s) represents the introduced Lyapunov function; After taking the derivative of the second function, we get: in is the control input for each AUV in the formation, W d The first derivative of (e,s), Indicates the virtual speed input v d The first derivative of ; Determine the control law: u a =R + (p)(-k v z+d v ) where k a ,k v >0,k a ,k v Respectively represent constant coefficients; R + (p) denotes the Moore-Penrose pseudoinverse of R(p), v m Indicates the speed of the fleet, v d represents the expected movement speed of the formation, u a represents the formation formation control input; The obstacle avoidance controller for reconstructing the multi-AUV three-dimensional formation is determined as: u=-k a s+h d -R T (p)z u a =R + (p)(-k v z+d v ) In the formula, k t is a positive constant, e t =p v -p1 is the position error between the virtual pilot AUV and AUV1, V v is the navigation speed of the virtual AUV, i.e., the planning path step length, k s is a positive constant and h d express, represents the first-order derivative of the virtual pilot AUV velocity vv, Indicates u a The first derivative of .

Images