Double-AUV target tracking path planning method and system based on broadside array detection

Abstract

The invention discloses a double-AUV (Autonomous Underwater Vehicle) target tracking path planning method and system based on broadside array detection, and belongs to the technical field of AUV target tracking path planning. The method comprises the following steps: establishing a double-AUV target tracking system model based on broadside array detection; the double AUVs detect target azimuth data through a broadside array; the double AUVs transmit target information revenue and pose data of the AUVs through communication; the double AUVs utilize extended information filtering to realize information state fusion estimation; converting the information state into a target estimation state and predicting a target state; the double AUVs utilize a particle swarm algorithm to realize expected path point planning based on a predicted target state; and respectively controlling each AUV to reach the expected path point. According to the method, continuous detection and tracking of the target can be realized under complex multi-constraint conditions such as actual broadside array detection constraint, double-AUV communication constraint and collision avoidance constraint, and excessive communication and calculation loads are not introduced while the detection effect is improved, so that the method has practical application significance.

Description

Technical Field

[0001] This invention belongs to the field of AUV target tracking path planning technology, specifically relating to a dual AUV target tracking path planning method and system based on flank array detection. Background Technology

[0002] Autonomous Underwater Vehicles (AUVs) have wide applications in marine science and engineering. One important application scenario for AUVs is the use of their onboard flank arrays for target detection and continuous tracking. However, flank arrays, as passive sonar, can only detect targets from their azimuth, resulting in weak observability for the tracking system. Effective tracking path planning is crucial to ensure target observability. Furthermore, the flank arrays, mounted on the port and starboard sides of the AUV, limit its field of view, creating blind spots at the bow and stern. Under these conditions of weak observability and limited field of view, a single AUV often struggles to meet the requirements for continuous target tracking, while multi-AUV collaborative tracking significantly increases communication and computational burdens. Dual AUVs offer a balanced solution, improving detection performance without introducing excessive communication and computational loads, thus possessing practical application value. Existing path planning methods for AUV detection and tracking rarely consider the maneuver constraints caused by the weak observability and limited field of view of flank arrays in real-world applications, which can easily lead to target loss. In reality, dual AUV tracking path planning faces complex multi-constraint conditions such as detection constraints, communication constraints, and collision avoidance constraints. Under this background, realizing dual AUV target detection and tracking path planning is extremely challenging and there is an urgent practical need.

[0003] Patent application number 202210386675.9 discloses "A Path Planning Method for Autonomous Underwater Robots Based on Multi-Objective Improved Particle Swarm Optimization Algorithm," primarily addressing the shortcomings of traditional particle swarm optimization algorithms, such as long running time, slow convergence speed, and susceptibility to local optima, in order to achieve fast, economical, and safe AUV path planning. First, this patent employs a planning method based on multi-objective improved particle swarm optimization, which differs from the dual AUV target tracking path planning method of this invention. Second, this patent focuses on introducing a mutation operator on top of the standard particle swarm optimization algorithm, increasing population diversity to avoid the algorithm getting trapped in local optima. It shortens the planning time based on an adaptive dynamic adjustment method using inertial weights and learning factors, transforming the multi-objective function into a monotonic and consistent comprehensive fitness function to achieve optimal AUV path planning. This differs from the dual AUV target tracking path planning method of this invention, which focuses on addressing the weak observability of the hull-side array and limited field-of-view coupling.

[0004] Patent application number 202111593676.2 discloses a "method for UAV tracking a moving target," primarily addressing the problem of trajectory planning when a UAV tracks a moving target without a predetermined target point and needs to maintain a constant distance from the target. First, this patent employs a planning method based on an artificial potential field, which differs from the dual-AUV target tracking path planning method of this invention. Second, this patent focuses on calculating the distance between the UAV at the current moment and the moving target at a future moment, determining a temporary target point based on the UAV's location on the confrontation circle and its speed, and using the artificial potential field method to plan the UAV's trajectory to the temporary target point. This differs from the dual-AUV target tracking path planning method of this invention, which focuses on addressing the weak observability of the flank array and the coupling of a limited field of view. Summary of the Invention

[0005] The purpose of this invention is to provide a method and system for dual AUV target tracking path planning based on flank array detection, addressing the problem of dual AUV target tracking path planning under conditions of weak observability of flank arrays and limited field-of-view coupling.

[0006] The objective of this invention is achieved through the following technical solution:

[0007] A dual AUV target tracking path planning method based on flank array detection, the specific steps of which are as follows:

[0008] Step 1: Establish a target tracking system model based on flank array detection;

[0009] Step 2: Dual AUVs detect target azimuth data via flank arrays. k represents the discrete time, and i∈{1,2} represents the AUV index;

[0010] Step 3: Dual AUVs transmit information status and benefits via underwater acoustic communication Information Matrix Benefits Its own northeast direction position Self-orbiting in, and Indicates the northward and eastward positions of the i-th AUV;

[0011] Step 4: Each AUV uses extended information filtering to determine the information status based on local observation and received communication data. With information matrix Fusion estimation;

[0012] Step 5: Transform the information state into the target estimated state X k And predict the future state of the target. Where stp represents the prediction step size;

[0013] Step 6: Based on the predicted target state, each AUV uses the particle swarm optimization algorithm to plan the desired path points of the two AUVs;

[0014] Step 7: Control each AUV to move to the desired path point;

[0015] Step 8: Repeat steps 2 to 7 to achieve continuous target tracking and path planning until the task is completed.

[0016] Furthermore, step 1 specifically includes:

[0017] Establish the target state transition model:

[0018] X k =F k|k-1 X k-1 +W k

[0019] Among them, F k|k-1 W represents the target state transition model. k Let Q represent the covariance matrix. k Gaussian process noise; Let x represent the state of the target at time k. k and Indicates the target's position and velocity in the north direction, y k and X represents the target's position and velocity in the eastward direction. k-1 Let the state of the target be at time k-1;

[0020] Establish a pure azimuth detection model for the flank array:

[0021]

[0022] in, The variance of the side array of the i-th AUV is denoted as . Gaussian observation noise; Ω i Let X represent the detection field of view of the i-th AUV side array, when X k ∈Ω i When the target is within the field of view of the i-th AUV side array, the target's bearing observed by the i-th AUV side array is obtained. when When i ∈ {1,2}, it indicates that the target is not within the field of view of the side array detection. null indicates that no target information was observed; i ∈ {1,2} represents the AUV number.

[0023] Furthermore, the information state benefit in step 3 and information matrix benefits for:

[0024]

[0025] Where, x k|k-1 and y k|k-1 Indicates the predicted target state X k|k-1 The corresponding target positions to the north and east, For linear observation matrices, and This represents the north and east positions of the i-th AUV. This represents the target bearing observed by the flank array of the i-th AUV. Let represent the variance of the observation noise of the i-th AUV side array.

[0026] Further, step 4 includes:

[0027] Step 4.1: Calculate the prediction information matrix ∑ k|k-1 and information status μ k|k-1 :

[0028] ∑ k|k-1 =[F k|k-1 (∑ k-1 ) -1 F k|k-1 T +Q k ] -1

[0029] μ k|k-1 =Σ k|k-1 F k|k-1 X k-1

[0030] Where, ∑ k-1 F represents the information matrix at time k-1. k|k-1 Represents the target state transition model, Q k Let X be the process noise covariance matrix at time k. k-1 This represents the state of the target at time k-1;

[0031] Step 4.2: Utilizing Local Information for Benefits Benefits from receiving neighbor information through communication The information matrix and information status are merged and updated;

[0032]

[0033] in, and These represent the information state gains of the two AUVs, respectively. and Let i and j represent the information matrix gains of the two AUVs, where i,j∈{1,2} and i≠j.

[0034] Furthermore, the target estimation state and the prediction of the future target state in step 5 are specifically as follows:

[0035] Final target estimated state X k for:

[0036] X k =(Σ k ) -1 μ k

[0037] Predicting future target state for:

[0038]

[0039] Where stp represents the prediction step size. F represents the predicted target state at time k+stp; k|k-1 Represents the target state transition model; X k Σ represents the state of the target at time k; k μ represents the information matrix at time k. k This represents the information state at time k.

[0040] Furthermore, step 6, which involves using the particle swarm optimization algorithm to plan the desired path points, specifically includes:

[0041] Step 6.1: Encode the particle position as the desired path point of the dual AUV;

[0042] Step 6.2: Initialize particle swarm parameters;

[0043] Step 6.3: Update the inertia weight ρ at time k k Particle velocity and particle position

[0044] Step 6.4: Calculate the current position of the particle using the fitness function. fitness value

[0045] fitness function

[0046] Current particle position fitness value

[0047] in, Indicates the predicted target location, x k+stp and y k+stp These represent the predicted northward and eastward positions of the target, respectively; This represents the cost of field-of-view constraints. This represents the cost of detection constraints. Indicates the communication constraint cost. Indicates the cost of steering maneuvers. This represents the cost of traversing to the opposite side; the smaller the fitness function value, the smaller the total cost, and the better the obtained particle position, reaching the optimum when the fitness value is 0.

[0048] Step 6.5: Update the historical best position of individual particles and fitness value like but Otherwise, proceed directly to step 6.6 without performing any operation;

[0049] Step 6.6: Update the historical best position of the particle population and fitness value like but Otherwise, proceed directly to step 6.7 without performing any operation;

[0050] Step 6.7: Determine the current iteration number (Iter) c Has the maximum number of iterations been reached? (Iter) c =Iter max Or meet the preset fitness threshold Or fitness value in 30 consecutive iterations If no change is made and the condition is met, proceed to step 6.8; otherwise, proceed to step 6.3.

[0051] Step 6.8: Output the desired path points for the AUV;

[0052] The best position in the history of the particle population is the optimal path point instruction set. Each AUV retrieves its desired position according to preset rules. AUV1 retrieves the first column of the path point instruction set as the desired path point, and AUV2 retrieves the second column of the path point instruction set as the desired path point.

[0053] Furthermore, the specific costs in step 6.4 are as follows:

[0054] (1) Field of view constraint cost

[0055]

[0056] in, Let i represent the side array field-of-view cost term for the i-th AUV, i∈{1,2}; when the target prediction position Falling into its flank array field of view Ω i Inner time, that is Will The value is set to 0 otherwise; the method for determining whether a target falls into the field of view of the i-th AUV side array is as follows:

[0057] For the i-th AUV, using particle position The i-th column (i.e., the expected position of the AUV) and the predicted position of the target Calculate the desired target relative azimuth angle

[0058]

[0059] if Within the effective opening angle of the flank array, i.e. This indicates that the target falls within the field of view of the i-th AUV side array, where α is the arc of the side array's end-fire blind zone. and Indicates the expected northward and expected eastward positions of the i-th AUV;

[0060] when This indicates that the target is within the field of view of both AUVs' side arrays and has not entered the end-firing blind zone;

[0061] (2) Detection constraint cost

[0062]

[0063] in, This represents the detection constraint cost term for the i-th AUV, where i ∈ {1, 2}; when the target prediction position Distance from AUV Exceeding the maximum detection range D of the flank array sonar At that time, that is Will The value is set to 1 otherwise; distance The calculation method is as follows:

[0064]

[0065] when This indicates that the target is within the maximum detection range of the two AUVs and has not been lost.

[0066] (3) Communication constraint cost

[0067]

[0068] Where i,j∈{1,2} represents the AUV index; when the distance between two AUVs Exceeding the maximum communication distance D commu At that time, that is Will The value is set to 1 otherwise; distance The calculation method is as follows:

[0069]

[0070] in, and Let i represent the expected northward and expected eastward positions of the j-th AUV, where i ≠ j;

[0071] when This indicates that the two AUVs are within communication range and communication has not been lost.

[0072] (4) Steering maneuver cost

[0073]

[0074] in, This represents the steering maneuver cost term for the i-th AUV, where i ∈ {1, 2}; when the desired heading of the AUV causes a change in heading. Exceeding the maximum heading change limit At that time, that is Will The value is set to 1 otherwise; the expected change in heading. The calculation method is as follows:

[0075]

[0076] when This indicates that both feasible commands for the AUV satisfy the steering constraints of the AUV.

[0077] (5) Cost of cross-border travel

[0078]

[0079] in, This represents the cost term for the i-th AUV's cross-traverse, where i ∈ {1, 2}; when the desired position of the AUV leads to the AUV's future path segment With the target future path segment Intersection, that is Will The value is set to 1 otherwise; AUV future path segment Current location of AUV To the desired position of the AUV A line segment (i.e., the i-th column corresponding to the particle position), representing the target's future path segment. Indicates the target's current estimated position (x) k ,y k ) to the target predicted location A line segment; the specific method for determining this is as follows:

[0080] like or or or

[0081] but otherwise,

[0082] like

[0083]

[0084] or

[0085]

[0086] but otherwise

[0087] when This indicates that no feasible position command for either AUV resulted in an AUV crossing from one side of the target to the other, thus satisfying the collision avoidance requirements for dual AUVs and preventing the target from entering the blind zone of the AUV's side-mounted array.

[0088] Furthermore, the specific steps for encoding particles in step 6.1 are as follows:

[0089] The feasible path point instructions for dual AUVs are encoded as particle positions. Each feasible position of a particle represents a set of feasible path point instructions for dual AUVs that meets the requirements of the target tracking task. The form is as follows:

[0090]

[0091] Each column represents the desired path point for an AUV. and This indicates the expected northward and expected eastward positions of AUV1. and Indicates the expected northward and expected eastward positions of AUV2;

[0092] The particle has velocity, and its position is continuously updated in the feasible solution space in order to find the optimal position; the feasible solution space is set as the predicted position of the target. Centered on the maximum detection range D of the AUV's flank array sonar The region with radius .

[0093] Furthermore, step 6.2, initializing the particle swarm parameters, specifically includes:

[0094] Initialize the maximum number of iterations Iter max Initialize the population particle number N sizeInitialize the historical best position of each individual particle. and fitness value Initialize the historical best position of the particle population and fitness value Initialize the maximum particle velocity Vel max Initialize learning factors λ1 and λ2, and initialize the maximum inertia weight ρ. max and minimum value ρ min Where n∈{1,2,…,N} size} represents the particle number.

[0095] A computer device / equipment / system includes a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of a dual AUV target tracking path planning method based on flank array detection.

[0096] The beneficial effects of this invention are as follows:

[0097] This invention effectively solves the target tracking problem under conditions of weak observability of the flank array and limited field-of-view coupling by performing multi-constraint collaborative planning on the target tracking path of dual AUVs. It achieves target state estimation by fusing the observation information gains of the dual AUVs through extended information filtering, and plans the desired path points of the dual AUVs based on the predicted target position using a particle swarm optimization algorithm. Ultimately, it enables continuous target detection and tracking by the dual AUVs while ensuring no target loss, maintaining mutual communication, and avoiding collisions. This method can achieve continuous target detection and tracking under complex multi-constraint conditions such as actual flank array detection constraints, dual AUV communication constraints, and collision avoidance constraints. Furthermore, it improves detection performance without introducing excessive communication and computational load, making it practically applicable. Attached Figure Description

[0098] Figure 1 This is a flowchart of a dual AUV target tracking path planning method based on flank array detection according to the present invention;

[0099] Figure 2 This is a schematic diagram of dual AUV target tracking based on flank array detection.

[0100] Figure 3 This is a flowchart of the information state fusion estimation process based on extended information filtering;

[0101] Figure 4 This is a flowchart of path planning based on the particle swarm optimization algorithm.

[0102] Figure 5 The diagram shows the effect of the dual AUV target tracking path planning based on the flank array detection method of the present invention.

[0103] Figure 6 The real-time distance between the AUV and the target during the path planning process of implementing the method of this invention for dual AUVs;

[0104] Figure 7 Real-time relative position between the AUV and the target during the path planning process of implementing the method of the present invention for dual AUVs;

[0105] Figure 8 The real-time distance between the two AUVs during the path planning process of implementing the method of the present invention for dual AUVs. Detailed Implementation

[0106] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0107] according to Figure 1 The present invention discloses a dual AUV target tracking path planning method based on flank array detection, comprising the following steps:

[0108] Step 1: Establish a target tracking system model based on flank array detection; specifically including:

[0109] Step 1.1: Establish the target state transition model as follows:

[0110] X k =F k|k-1 X k-1 +W k

[0111] Where k represents discrete time, F k|k-1 W represents the target state transition model. k Let Q represent the covariance matrix. k Gaussian process noise; Let x represent the state of the target at time k. k and Indicates the target's position and velocity in the north direction, y k and X represents the target's position and velocity in the eastward direction. k-1 Let the state of the target be at time k-1;

[0112] Step 1.2: Establish a pure azimuth detection model for the flank array; for details, please refer to... Figure 2 The flank array can only detect the target's azimuth, and its field of view is limited by the end-firing blind zone, making it impossible to observe targets at the bow and stern of the AUV; its detection model is as follows:

[0113]

[0114] in, and This represents the north and east positions of the i-th AUV. The variance of the side array of the i-th AUV is denoted as . Gaussian observation noise; Ω i Let X represent the detection field of view of the i-th AUV side array, when X k ∈Ω i When the target is within the field of view of the i-th AUV side array, the target's bearing observed by the i-th AUV side array is obtained. when When i ∈ {1,2}, it indicates that the target is not within the field of view of the flank array detection; null indicates that no target information was observed; i ∈ {1,2} represents the AUV number.

[0115] Step 1.3: Calculate the linear observation matrix for:

[0116]

[0117] Step 2: Dual AUVs detect target azimuth data via flank arrays. k represents the discrete time, and i∈{1,2} represents the AUV index; specifically, such as Figure 2 As shown, AUV1 obtains the target's azimuth within the field of view of the flank array detection system. The AUV2 obtains the target's bearing within the field of view of the flank array detection system.

[0118] Step 3: The dual AUVs transmit target information, revenue data, and their own pose data via underwater acoustic communication; specifically, such as... Figure 3 As shown, the transmitted data specifically includes: information status benefits. Information Matrix Benefits Its own northeast direction position Self-orbiting

[0119] Step 4: Each AUV uses extended information filtering based on local observation and communication data to achieve information state fusion estimation; specifically, such as... Figure 3 As shown, each AUV achieves fusion by utilizing local information gains and target information gains from another AUV received via communication; the extended information filtering specifically includes:

[0120] Step 4.1: Predict the information matrix ∑ k|k-1 and information status μ k|k-1 :

[0121] ∑ k|k-1=[F k|k-1 (∑ k-1 ) -1 F k|k-1 T +Q k ] -1

[0122] μ k|k-1 =Σ k|k-1 F k|k-1 X k-1

[0123] Where, ∑ k-1 F represents the information matrix at time k-1. k|k-1 Represents the target state transition model, Q k Let X be the process noise covariance matrix at time k. k-1 This represents the state of the target at time k-1;

[0124] Information status μ k-1 Information matrix Σ k-1 and target state X k-1 Target state covariance matrix P k-1 The conversion relationship between them is as follows:

[0125] ∑ k-1 =(P k-1 ) -1

[0126] μ k-1 =Σ k-1 X k-1

[0127] Among them, P k-1 This represents the target state covariance matrix at time k-1;

[0128] Step 4.2: Calculate local information revenue;

[0129] Information status benefits for:

[0130]

[0131] Among them, the target state X is predicted. k|k-1 for:

[0132] X k|k-1 =F k|k-1 X k-1

[0133] Information Matrix Benefits for:

[0134]

[0135] Where, xk|k-1 and y k|k-1 Indicates the predicted target state X k|k-1 The corresponding target positions to the north and east, For linear observation matrices, and This represents the north and east positions of the i-th AUV. This represents the target bearing observed by the flank array of the i-th AUV. Let represent the variance of the observation noise of the i-th AUV side array.

[0136] Step 4.3: Merge and update; specifically, such as Figure 3 As shown, the benefits of utilizing local information Benefits from receiving neighbor information through communication The information matrix and information status are merged and updated;

[0137]

[0138] in, and These represent the information state gains of the two AUVs, respectively. and Let i and j represent the information matrix gains of the two AUVs, where i,j∈{1,2} and i≠j.

[0139] Step 5: Transform the information state into the target estimated state X k And predict the future state of the target based on the model. Specifically:

[0140] Final target estimated state X k for:

[0141] X k =(Σ k ) -1 μ k

[0142] Predicting future target state for:

[0143]

[0144] Where stp represents the prediction step size. The target state at time k+stp, as predicted, is in the following form:

[0145]

[0146] Where, x k+stp , y k+stp and Representing the predicted state The corresponding northward position, northward velocity, eastward position, and eastward velocity, F k|k-1 Represents the target state transition model; X k Σ represents the state of the target at time k; k μ represents the information matrix at time k. k This represents the information state at time k.

[0147] Step 6: Each AUV plans its desired path points using the particle swarm optimization algorithm based on the predicted target state; for details, please refer to [link to relevant documentation]. Figure 4 The particle swarm optimization algorithm plans the desired path points as follows:

[0148] Step 6.1: Encode the particle position as the desired path point of the dual AUV; specifically:

[0149] The feasible path point instructions for dual AUVs are encoded as particle positions. Each feasible position of a particle represents a set of feasible path point instructions for dual AUVs that meets the requirements of the target tracking task. The form is as follows:

[0150]

[0151] Each column represents the desired path point for an AUV. and This indicates the expected northward and expected eastward positions of AUV1. and Indicates the expected northward and expected eastward positions of AUV2;

[0152] The particle has velocity, and its position (i.e., a feasible solution to the problem) is continuously updated in the feasible solution space to find the optimal position; the feasible solution space is set as the predicted position of the target. Centered on the maximum detection range D of the AUV's flank array sonar A region with a radius of ;

[0153] Step 6.2: Initialize particle swarm parameters; specifically including: initializing the maximum number of iterations (Iter). max Initialize the population particle number N size Initialize the historical best position of each individual particle. and fitness value Initialize the historical best position of the particle population and fitness value Initialize the maximum particle velocity Vel max Initialize learning factors λ1 and λ2, and initialize the maximum inertia weight ρ. max and minimum value ρ min Where n∈{1,2,…,N} size} represents the particle number;

[0154] Step 6.3: Update the inertia weight ρ at time k k Particle velocity and particle position Specifically, it includes:

[0155] The updated weights are:

[0156]

[0157] Among them, Iter c This represents the current iteration number;

[0158] Update particle velocity as follows:

[0159]

[0160] Where r1 and r2 are random numbers in the range [0,1];

[0161] if but

[0162] Update particle positions as follows:

[0163]

[0164] in, Let K be the particle velocity at time k-1. Let be the particle position at time k-1;

[0165] Step 6.4: Utilize the fitness function Calculate the current position of the particle fitness value in Indicates the predicted target location;

[0166] Specifically, the fitness value The calculation method is as follows:

[0167]

[0168] Specifically, the fitness function is as follows:

[0169]

[0170] In the fitness function, This represents the cost of field-of-view constraints. This represents the cost of detection constraints. Indicates the communication constraint cost. Indicates the cost of steering maneuvers. This represents the cost of traversing to the opposite side; the smaller the fitness function value, the smaller the total cost, and the better the obtained particle position, reaching the optimum when the fitness value is 0; the specific costs are as follows:

[0171] (1) Field of view constraint cost

[0172]

[0173] in, Let i represent the side array field-of-view cost term for the i-th AUV, i∈{1,2}; when the target prediction position Falling into its flank array field of view Ω i Inner time, that is Will The value is set to 0 otherwise; the method for determining whether a target falls into the field of view of the i-th AUV side array is as follows:

[0174] For the i-th AUV, using particle position The i-th column (i.e., the expected position of the AUV) and the predicted position of the target Calculate the desired target relative azimuth angle

[0175]

[0176] if Within the effective opening angle of the flank array, i.e. This indicates that the target falls within the field of view of the i-th AUV side array, where α is the arc of the side array's end-fire blind zone. and Indicates the expected northward and expected eastward positions of the i-th AUV;

[0177] when This indicates that the target is within the field of view of both AUVs' side arrays and has not entered the end-firing blind zone;

[0178] (2) Detection constraint cost

[0179]

[0180] in, This represents the detection constraint cost term for the i-th AUV, where i ∈ {1, 2}; when the target prediction position Distance from AUV Exceeding the maximum detection range D of the flank array sonar At that time, that is Will The value is set to 1 otherwise; distance The calculation method is as follows:

[0181]

[0182] when This indicates that the target is within the maximum detection range of the two AUVs and has not been lost.

[0183] (3) Communication constraint cost

[0184]

[0185] Where i,j∈{1,2} represents the AUV index; when the distance between two AUVs Exceeding the maximum communication distance D commu At that time, that is Will The value is set to 1 otherwise; distance The calculation method is as follows:

[0186]

[0187] in, and Let i represent the expected northward and expected eastward positions of the j-th AUV, where i ≠ j;

[0188] when This indicates that the two AUVs are within communication range and communication has not been lost.

[0189] (4) Steering maneuver cost

[0190]

[0191] in, This represents the steering maneuver cost term for the i-th AUV, where i ∈ {1, 2}; when the desired heading of the AUV causes a change in heading. Exceeding the maximum heading change limit At that time, that is Will The value is set to 1 otherwise; the expected change in heading. The calculation method is as follows:

[0192]

[0193] when This indicates that both feasible commands for the AUV satisfy the steering constraints of the AUV.

[0194] (5) Cost of cross-border travel

[0195]

[0196] in, This represents the cost term for the i-th AUV's cross-traverse, where i ∈ {1, 2}; when the desired position of the AUV leads to the AUV's future path segment With the target future path segment Intersection, that is Will The value is set to 1 otherwise; AUV future path segment Current location of AUV To the desired position of the AUV A line segment (i.e., the i-th column corresponding to the particle position), representing the target's future path segment. Indicates the target's current estimated position (x) k ,y k ) to the target predicted location A line segment; the specific method for determining this is as follows:

[0197] like or or or

[0198] but otherwise,

[0199] like

[0200]

[0201] or

[0202]

[0203] but otherwise

[0204] when This indicates that no feasible position commands for either AUV resulted in the AUV crossing from one side of the target to the other, thus meeting the collision avoidance requirements for dual AUVs and preventing the target from entering the blind zone of the AUV's side-mounted array.

[0205] Step 6.5: Update the historical best position of individual particles and fitness value like but Otherwise, proceed directly to step 6.6 without performing any operation;

[0206] Step 6.6: Update the historical best position of the particle population and fitness value like but Otherwise, proceed directly to step 6.7 without performing any operation;

[0207] Step 6.7: Determine if the termination condition is met. If it is, proceed to step 6.8; otherwise, go to step 6.3. The termination condition is: reaching the maximum number of iterations (Iter). c =Iter max Or meet the preset fitness threshold Or fitness value in 30 consecutive iterations No change;

[0208] Step 6.8: Output the expected path point of AUV; specifically: the best position in the history of the particle population is the optimal path point instruction set. Each AUV extracts its own expected position according to the preset rules. AUV1 obtains the first column of the path point instruction set as the expected path point, and AUV2 obtains the second column of the path point instruction set as the expected path point.

[0209] Step 7: Control each AUV to reach the desired path point.

[0210] Step 8: Repeat steps 2 to 7 to achieve continuous target tracking and path planning until the task is completed.

[0211] Example:

[0212] Figure 5 , Figure 6 , Figure 7 and Figure 8 A Monte Carlo simulation example of the dual AUV target tracking path planning method based on flank array detection in this invention is demonstrated. The example uses dual AUVs tracking a target moving at a constant velocity in a straight line. A total of 500 Monte Carlo simulations were performed, with a simulation sampling time of ΔT = 0.5 s; the number of particles in the population was set to N. size =200, the maximum number of iterations for particle swarm optimization is set to Iter max =500, the maximum value of the inertia weight is set to ρ max =0.9, minimum value set as ρ min =0.4, maximum particle velocity set to Vel max =5, learning factors are set to λ1=1 and λ2=1; the initial target state is X0=[0m,3.3m / s,500m,3.3m / s] T The initial position of AUV1 is (-370, 780), the position of AUV2 is (450, 100), the maximum detection range of the AUVs is 750 meters, the maximum communication range of the AUVs is 1500 meters, the blind zone angle of the flank array is α = π / 9, and the target state transition model F k|k-1 for:

[0213]

[0214] Figure 5 The diagram shows the effect of the dual AUV target tracking path planning based on the flank array detection method of the present invention. Figure 6 The real-time distance between the AUV and the target during the path planning process of implementing the method of this invention for dual AUVs; Figure 7 Real-time relative position between the AUV and the target during the path planning process of implementing the method of the present invention for dual AUVs; Figure 8 Real-time distance between two AUVs during the path planning process of implementing the method of the present invention for dual AUVs;

[0215] exist Figure 5 In this paper, the method of the present invention is applied to realize the target tracking path planning of dual AUVs based on the side array detection. As can be seen from the figure, the path between the two AUVs is collision-free, and the path of each AUV does not cross from one side of the target to the other. As can be seen from the detection details shown in the figure, the target is within the detection field of view of the dual AUVs, and the estimated target path is close to the actual target path.

[0216] exist Figure 6 As can be seen, the real-time distances between AUV1 and AUV2 and the target are both less than the maximum detection range of the flank array; Figure 7 As can be seen, the real-time relative positions of AUV1 and AUV2 with respect to the target are both within the field of view of the flank array detection system; Figure 6 and Figure 7 Both sides stated that during the tracking process, the target remained within the field of view and maximum detection range of the side array detection of AUV1 and AUV2, and there was no instance of the target being lost.

[0217] exist Figure 8 As can be seen, the real-time distance between AUV1 and AUV2 is always less than the maximum communication distance, and the dual AUVs meet the real-time communication requirements during target tracking.

[0218] In particular, in some preferred embodiments of the present invention, a computer device is also provided, including a memory and a processor and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the dual AUV target tracking path planning method based on flank array detection described in any of the above embodiments.

[0219] In some other preferred embodiments of the present invention, a computer-readable storage medium is also provided, on which a computer program / instruction is stored, wherein when the computer program is executed by a processor, the steps of the dual AUV target tracking path planning method based on flank array detection described in any of the above embodiments are implemented.

[0220] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the above embodiments of the dual AUV target tracking path planning method based on flank array detection, which will not be repeated here.

[0221] Computer-readable storage media encompass a variety of types, including persistent and non-persistent, portable and fixed. These media store information using different technologies, and the content can be machine instructions, data structures, program modules, or other types of data. Some typical examples of computer storage media include: phase-change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), various types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory and other storage technologies, optical storage media such as CD-ROM and digital video disc (DVD), magnetic storage devices such as magnetic tape and disks, and other non-transferable media used to store information accessible to computing devices. It is important to note that the computer-readable media described herein do not include temporary storage media, such as modulated data signals and carrier waves.

[0222] Those skilled in the art will further recognize that the operation of the module can be achieved using existing technical protocols or programs, without relying on new computer programs themselves. The units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the components and steps of the various examples have been generally described in terms of functionality in the foregoing description. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0223] The steps of the methods or algorithms described in conjunction with the embodiments disclosed herein can be implemented in hardware, a software module executed by a processor, or a combination of both. The software module can be located in random access memory (RAM), main memory, read-only memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, removable disk, CD-ROM, or any other form of storage medium known in the art.

[0224] The present invention provides a dual AUV target tracking path planning method and system based on flank array detection, which can solve the target tracking path planning problem under the conditions of weak observability of flank array and limited field of view coupling. It effectively realizes continuous target detection and tracking under complex multi-constraint conditions such as actual flank array detection constraints, dual AUV communication constraints and collision avoidance constraints, without the occurrence of target loss, collision between AUVs, or loss of communication between AUVs.

[0225] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A dual AUV target tracking path planning method based on flank array detection, characterized in that: The specific steps are as follows: Step 1: Establish a target tracking system model based on flank array detection; Step 2: Dual AUVs detect target azimuth data via flank arrays. k represents the discrete time, and i∈{1,2} represents the AUV index; Step 3: Dual AUVs transmit information status and benefits via underwater acoustic communication Information Matrix Benefits Its own northeast direction position Self-orbiting in, and Indicates the northward and eastward positions of the i-th AUV; Step 4: Each AUV uses extended information filtering to determine the information status based on local observation and received communication data. With information matrix Fusion estimation; Step 5: Transform the information state into the target estimated state X k And predict the future state of the target. Where stp represents the prediction step size; Step 6: Based on the predicted target state, each AUV uses the particle swarm optimization algorithm to plan the desired path points of the two AUVs; Step 7: Control each AUV to move to the desired path point; Step 8: Repeat steps 2 to 7 to achieve continuous target tracking and path planning until the task is completed.

2. The dual AUV target tracking path planning method based on flank array detection according to claim 1, characterized in that: Step 1 specifically includes: Establish the target state transition model: X k =F k|k-1 X k-1 +W k Among them, F k|k-1 W represents the target state transition model. k Let Q represent the covariance matrix. k Gaussian process noise; Let x represent the state of the target at time k. k and Indicates the target's position and velocity in the north direction, y k and X represents the target's position and velocity in the eastward direction. k-1 Let the state of the target be at time k-1; Establish a pure azimuth detection model for the flank array: in, The variance of the side array of the i-th AUV is denoted as . Gaussian observation noise; Ω i Let X represent the detection field of view of the i-th AUV side array, when X k ∈Ω i When the target is within the field of view of the i-th AUV side array, the target's bearing observed by the i-th AUV side array is obtained. when When the value is 0, it indicates that the target is not within the field of view of the flank array detection. null indicates that no target information was observed.

3. The dual AUV target tracking path planning method based on flank array detection according to claim 2, characterized in that: Information status benefits in step 3 and information matrix benefits for: Where, x k|k-1 and y k|k-1 Indicates the predicted target state X k|k-1 The corresponding target positions to the north and east, For linear observation matrices, Let represent the variance of the observation noise of the i-th AUV side array.

4. The dual AUV target tracking path planning method based on flank array detection according to claim 3, characterized in that: Step 4 includes: Step 4.1: Calculate the prediction information matrix ∑ k|k-1 and information status μ k|k-1 : m k|k-1 =S k|k-1 F k|k-1 X k-1 Where, ∑ k-1 F represents the information matrix at time k-1. k|k-1 Let X represent the target state transition model. k-1 This represents the state of the target at time k-1; Step 4.2: Utilizing Local Information for Benefits Benefits from receiving neighbor information through communication The information matrix and information status are merged and updated; in, and These represent the information state gains of the two AUVs, and Let i and j represent the information matrix gains of the two AUVs, where i,j∈{1,2} and i≠j.

5. The dual AUV target tracking path planning method based on flank array detection according to claim 4, characterized in that: The target estimation state and the prediction of the future target state in step 5 are specifically as follows: Final target estimated state X k for: X k =(Σ k ) -1 m k Predicting future target state for: in, F represents the predicted target state at time k+stp; k|k-1 Represents the target state transition model; X k Σ represents the state of the target at time k; k μ represents the information matrix at time k. k This represents the information state at time k.

6. The dual AUV target tracking path planning method based on flank array detection according to claim 1, characterized in that: Step 6, which involves planning the desired path points using the particle swarm optimization algorithm, specifically includes: Step 6.1: Encode the particle position as the desired path point of the dual AUV; Step 6.2: Initialize particle swarm parameters; Step 6.3: Update the inertia weight ρ at time k k Particle velocity and particle position Step 6.4: Calculate the current position of the particle using the fitness function. fitness value fitness function Current particle position fitness value in, Indicates the predicted target location, x k+stp and y k+stp These represent the predicted northward and eastward positions of the target, respectively; This represents the cost of field-of-view constraints. This represents the cost of detection constraints. Indicates the communication constraint cost. Indicates the cost of steering maneuvers. This represents the cost of traversing to the opposite side; the smaller the fitness function value, the smaller the total cost, and the better the obtained particle position, reaching the optimum when the fitness value is 0. Step 6.5: Update the historical best position of individual particles and fitness value like but Otherwise, proceed directly to step 6.6 without performing any operation; Step 6.6: Update the historical best position of the particle population and fitness value like but Otherwise, proceed directly to step 6.7 without performing any operation; Step 6.7: Determine the current iteration number (Iter) c Has the maximum number of iterations been reached? (Iter) c =Iter max Or meet the preset fitness threshold Or fitness value in 30 consecutive iterations If no change is made and the condition is met, proceed to step 6.8; otherwise, proceed to step 6.

3. Step 6.8: Output the desired path points for the AUV; The best position in the history of the particle population is the optimal path point instruction set. Each AUV retrieves its desired position according to preset rules. AUV1 obtains the first column of the path point instruction set as the desired path point, and AUV2 obtains the second column of the path point instruction set as the desired path point.

7. The dual AUV target tracking path planning method based on flank array detection according to claim 6, characterized in that: The specific costs in step 6.4 are as follows: (1) Field of view constraint cost in, Let i represent the side array field-of-view cost term for the i-th AUV, i∈{1,2}; when the target prediction position Falling into its flank array field of view Ω i Inner time, that is Will The value is set to 0 otherwise; the method for determining whether a target falls into the field of view of the i-th AUV side array is as follows: For the i-th AUV, using particle position The i-th column (i.e., the expected position of the AUV) and the predicted position of the target Calculate the desired target relative azimuth angle if Within the effective opening angle of the flank array, i.e. This indicates that the target falls within the field of view of the i-th AUV side array, where α is the arc of the side array's end-fire blind zone. and Indicates the expected northward and expected eastward positions of the i-th AUV; when This indicates that the target is within the field of view of both AUVs' side arrays and has not entered the end-firing blind zone; (2) Detection constraint cost in, This represents the detection constraint cost term for the i-th AUV, where i ∈ {1, 2}; when the target prediction position Distance from AUV Exceeding the maximum detection range D of the flank array sonar At that time, that is Will The value is set to 1 otherwise; distance The calculation method is as follows: when This indicates that the target is within the maximum detection range of the two AUVs and has not been lost. (3) Communication constraint cost Among them, when the distance between the two AUVs Exceeding the maximum communication distance D commu At that time, that is Will The value is set to 1 otherwise; distance The calculation method is as follows: Where i ≠ j; when This indicates that the two AUVs are within communication range and communication has not been lost. (4) Steering maneuver cost in, This represents the steering maneuver cost term for the i-th AUV, where i ∈ {1, 2}; when the desired heading of the AUV causes a change in heading. Exceeding the maximum heading change limit At that time, that is Will The value is set to 1 otherwise; the expected change in heading. The calculation method is as follows: when This indicates that both feasible commands for the AUV satisfy the steering constraints of the AUV. (5) Cost of cross-border travel in, This represents the cost term for the i-th AUV's cross-traverse, where i ∈ {1, 2}; when the desired position of the AUV leads to the AUV's future path segment With the target future path segment Intersection, that is Will The value is set to 1 otherwise; AUV future path segment Current location of AUV To the desired position of the AUV That is, a line segment in the i-th column corresponding to the particle's position, representing the target's future path segment. Indicates the target's current estimated position (x) k ,y k ) to the target predicted location A line segment; the specific method for determining this is as follows: like or or or but otherwise, like or but otherwise when This indicates that no feasible position command for either AUV resulted in an AUV crossing from one side of the target to the other, thus satisfying the collision avoidance requirements for dual AUVs and preventing the target from entering the blind zone of the AUV's side-mounted array.

8. The dual AUV target tracking path planning method based on flank array detection according to claim 6, characterized in that: The specific process of encoding particles in step 6.1 is as follows: The feasible path point instructions for dual AUVs are encoded as particle positions. Each feasible position of a particle represents a set of feasible path point instructions for dual AUVs that meets the requirements of the target tracking task. The form is as follows: Each column represents the desired path point for an AUV. and This indicates the expected northward and expected eastward positions of AUV1. and Indicates the expected northward and expected eastward positions of AUV2; The particle has velocity, and its position is continuously updated in the feasible solution space in order to find the optimal position; the feasible solution space is set as the predicted position of the target. Centered on the maximum detection range D of the AUV's flank array sonar The region with radius .

9. The dual AUV target tracking path planning method based on flank array detection according to claim 6, characterized in that: Step 6.2, initializing the particle swarm parameters, specifically includes: Initialize the maximum number of iterations Iter max Initialize the population particle number N size Initialize the historical best position of each individual particle. and fitness value Initialize the historical best position of the particle population and fitness value Initialize the maximum particle velocity Vel max Initialize learning factors λ1 and λ2, and initialize the maximum inertia weight ρ. max and minimum value ρ min Where n∈{1,2,…,N} size } represents the particle number.

10. A computer device / equipment / system, comprising a memory, a processor, and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 9.

Images