An optimal path planning method for autonomous surface vehicles with minimum target positioning error

Abstract

This invention proposes an optimal path planning method for surface autonomous vehicles (AUVs) that minimizes target positioning error. The method includes: S1, constructing a system model of the AUV including a motion model and an observation model; S2, deriving the optimal geometric relationship between the AUV and the target; S3, obtaining the target's prior position information for one or two AUVs using a gridded region approach, or for three AUVs using positioning technology; S4, using a Monte Carlo strategy to particleize nonlinear non-Gaussian ranging noise and constructing a closed-loop expression for the objective function; and S5, solving the objective function using a nonlinear dynamic programming algorithm to obtain the optimal path. Its advantage is that it solves the problem of finding the optimal path planning method for surface vehicles with the minimum positioning error when the target's prior position information is unknown and nonlinear non-Gaussian noise exists, making it impossible to obtain the closed-loop expression of the objective function.

Description

Technical Field

[0001] This invention relates to the field of path planning for autonomous surface vehicles, and specifically to an optimal path planning method for autonomous surface vehicles that minimizes target positioning error. Background Technology

[0002] As a crucial tool for ocean exploration, autonomous underwater vehicles (AUVs) are widely used in scientific and commercial missions, such as marine resource tracking, seabed mapping, and marine monitoring. In these missions, data is meaningless without its location coordinates. However, the underwater environment cannot receive GPS signals. Therefore, how to better locate AUVs (targets) using autonomous surface vehicles (ASVs) is a pressing problem to be solved.

[0003] Existing technologies have a significant drawback: they only consider cases where noise during signal propagation follows a Gaussian distribution. However, the marine environment is complex and variable. During signal propagation, especially underwater, it is highly susceptible to nonlinear, non-Gaussian noise generated by factors such as plumes and eddies. Consequently, existing technologies are ineffective at target localization, leading to inaccurate positioning. Furthermore, existing technologies for target localization using ASVs often rely on a known initial target position, which is difficult to achieve in the highly dynamic marine environment, resulting in inaccurate initial target positions. Therefore, finding a better way to utilize ASVs for target localization, plan the optimal path, and minimize positioning errors under conditions of nonlinear, non-Gaussian noise and uncertain initial target positions presents a significant challenge. Summary of the Invention

[0004] The purpose of this invention is to provide an optimal path planning method for a surface autonomous vehicle with the minimum target positioning error. This method addresses the problem that, when the prior information of the target position is unknown and nonlinear non-Gaussian noise exists, it is impossible to obtain the closed-loop expression of the objective function, which in turn makes it impossible to solve the optimal path planning problem for a surface vehicle with the minimum positioning error.

[0005] To achieve the above objectives, the present invention is implemented through the following technical solution:

[0006] An optimal path planning method for an autonomous surface vehicle with minimal target positioning error is characterized by comprising the following steps:

[0007] S1. Construct a system model (motion model and observation model) for the autonomous surface vehicle;

[0008] S2. The optimal geometric relationship between the autonomous surface vehicle and the target is derived.

[0009] S3. Obtain the target's prior location information through a gridded area method (for one or two autonomous surface vehicles) or positioning technology (for three autonomous surface vehicles);

[0010] S4. Utilize the Monte Carlo strategy to particleize nonlinear non-Gaussian ranging noise and construct a closed-loop expression for the objective function;

[0011] S5. Solve the objective function using a nonlinear dynamic programming algorithm to obtain the optimal path.

[0012] Step S1, which involves constructing the system model (motion model and observation model) of the autonomous surface vehicle, specifically includes:

[0013] S11. Considering the known water depth of the target, the 3D scene is mapped to 2D for resolution. Assume the coordinate reference system is an East-North-Down geodetic coordinate system, and the target's position at time t is... The position of the i-th ASVs at time t is Where T represents transpose. If ASVs can sense its azimuth angle φ(t), consider its input velocity v(t) and angular velocity r. i (t), then the state of the corresponding ASVs can be represented as

[0014]

[0015] Define two orthogonal unit vectors, i.e.

[0016]

[0017] The motion model of ASVs without considering flow velocity is as follows:

[0018] in, and φ represents the position of the i-th ASVs at the previous time and at the current time, respectively; i (t-1) and φ i (t) represents the azimuth information of the i-th ASVs at the previous time and at the current time, respectively; when the angular velocity is not 0, it indicates that the ASVs are moving in a curve during the corresponding time period, and conversely, when the angular velocity is 0, it means that the ASVs are moving in an approximately straight line.

[0019] S12 and ASVs are equipped with sensors capable of receiving signal strength. By acquiring their signal strength values, their positions relative to the target are calculated. The system's observation equations are further constructed using a path loss model.

[0020]

[0021] in, Pi represents the signal power value received by the i-th ASVs from the target at time t; P0 represents the power value transmitted by the target, i.e., assuming that the transmitted power value is equal at every time. Indicates the path loss factor; d represents the distance between the i-th ASV and the target at time t; d0 represents the reference distance, which is usually 1m; This indicates that the corresponding Gaussian noise is assumed to be equal at every moment. Its mean is zero and its variance is σ.

[0022] Due to the complex and variable marine environment, its noise is not only Gaussian noise but may also contain outliers, resulting in non-Gaussian noise characteristics. Introducing a noise pollution factor β, with a value range of [0,1), the probability distribution corresponding to non-Gaussian noise can be expressed as follows:

[0023] p(η)=(1-β)Ω(η;0,σ 2 )+βΦ(η), (5)

[0024] Wherein, Ω(η;0,σ 2 ) represents the Gaussian probability distribution; Φ(η) represents the probability distribution of non-Gaussian anomalous noise.

[0025] Step S2, which derives the optimal geometric relationship between the autonomous surface vehicle and the target, specifically includes:

[0026] S21. The optimal geometric relation exists if and only if the number of ASVs is greater than or equal to 2.

[0027] S22. For two ASVs scenarios, it is derived that the optimal geometric relationship is when the relative target angle is π / 2.

[0028] Corollary 1: Let α 12 Let α be the angle between ASVs 1 and ASVs 2, and let α be the angle between ASVs 1 and ASVs 2. 12 =α 21 For any fixed In other words, the angle that maximizes the determinant of the Fisher information matrix is ​​π / 2, i.e., α. 12 =α 21 =π / 2.

[0029] Proof 1: Let in And there exists a constant γ > 0 such that γ2 = ζγ1. At each time step, its corresponding function can be expressed as:

[0030]

[0031] To minimize the target positioning error, we need to maximize the determinant of equation (6), which is equivalent to minimizing equation (7), i.e.

[0032]

[0033] Through transformation, we can further obtain

[0034]

[0035] It is obvious that when cos(2φ1(t)-2φ2(t))=-1, equation (8) can reach its minimum value. Therefore, for In other words, For two ASVs, the optimal geometric relationship can be achieved when their angle is π / 2, i.e. Q.E.D.

[0036] S23. For a scenario with three ASVs, the optimal geometric angles relative to the target are derived as follows: 1) Two ASVs have an angle of 0 or π relative to the target, and the angle of the third ASV relative to these two ASVs is π / 2; 2) All three ASVs have an angle of 2π / 3 relative to the target; 3) Two ASVs have an angle of π / 3 relative to the target, and the angle of the third ASV relative to the target is 2π / 3. The derivation process is as follows:

[0037] Let α 12 α 13 and α 23 This represents the angular relationship between the i-th ASVs and the l-th ASVs, where i, l ∈ {1, 2, 3}. γ il =γ i ·γ l And α 12 =α 21 α 13 =α 31 α 23 =α 32 The angular relationship between the i-th ASVs and the l-th ASVs can be expressed as:

[0038]

[0039] If γ l >∑ 1≤i≤3,i≠l γ i This means that if an ASV is close to the target, then...

[0040]

[0041] Equation (10) can be further expressed as

[0042]

[0043] Here, \ represents the way the set is reduced.

[0044] When γ1 = γ2 = γ3, there exists a special optimal geometric relationship, namely α 12 =α 13 =α 23 =2π / 3 or α 12 =α 23 =π / 3, α 13 =2π / 3.

[0045] Step S3 obtains the target's prior location information through a gridded region method (for one or two autonomous surface vehicles) or positioning technology (for three autonomous surface vehicles), specifically including:

[0046] S31. When there are only 1 or 2 ASVs, quantify the uncertainty of the target's prior position, rasterize the relevant region, and use the intersections in the grid as the possible positions of the target, such as... Figure 2 As shown.

[0047] S32. When there are 3 ASVs, the prior location information of the target is obtained through positioning technology. If d0 = 1m, equation (4) in S12 can be obtained by transformation.

[0048]

[0049] Then, by expanding using a first-order Taylor series, we obtain...

[0050]

[0051] By performing a squared distance operation and combining it with weights, a nonlinear least squares expression is constructed, namely...

[0052]

[0053] in This indicates the corresponding estimated distance;

[0054] After expanding equation (14), it can be further transformed into a generalized trust domain subproblem, that is...

[0055]

[0056] Where y = [(B t ) T ,||B t || 2 ] T ;

[0057]

[0058] I and 0 represent the identity matrix and the zero matrix, respectively.

[0059] After obtaining the expression (15) of the generalized trust domain subproblem, the prior position of the target is obtained by solving the bisection method.

[0060] Step S4 utilizes a Monte Carlo strategy to particleize the nonlinear non-Gaussian ranging noise and constructs a closed-loop expression for the objective function, specifically including:

[0061] S41. To minimize the positioning estimation error, a Crommellow lower bound constructor from parameter estimation theory is introduced. The Fisher information matrix at time t is:

[0062]

[0063] Due to the presence of outliers, equation (17) cannot be expressed in a closed-loop manner. Therefore, by employing a Monte Carlo strategy to particleize the nonlinear non-Gaussian noise, equation (17) can be expressed as follows:

[0064]

[0065] in,

[0066]

[0067] ▽ η [p(η)] t The first-order gradient is represented by Equation (20), which is a particle-based operation using the Monte Carlo strategy to handle nonlinear non-Gaussian noise.

[0068] S42. For ASVs, to minimize the target localization error, it is necessary to maximize the determinant of the Fisher information matrix, i.e.

[0069]

[0070] in,

[0071] Taking the logarithm of equation (21), the objective function can be expressed as follows:

[0072]

[0073] As can be seen from equation (22), if angular velocity is taken as the variable, for the optimal path of ASVs, the parameter that needs to be solved at each moment is the corresponding angular velocity, i.e.

[0074]

[0075] in, represents the optimal angular velocity of the i-th ASVs within the time interval from 1 to M; c represents the corresponding constraint.

[0076] S43. When there is only one ASV, considering all the intersection points of the grid in the uncertain region as k, it is necessary to maximize the worst case in the uncertain region. Combining the set of all intersection points in the grid, according to the optimization equation of equation (23), the objective function for only one ASV can be expressed as follows:

[0077]

[0078] S44. When there are 2 ASVs, similarly considering all the intersection points of the grid in the uncertain region as k, it is necessary to maximize the worst case in the uncertain region. Combining the set of all intersection points in the grid, according to the optimization equation of equation (23), the objective function of 2 ASVs can be expressed as follows:

[0079]

[0080] S45. When there are 3 ASVs, the corresponding objective function is:

[0081]

[0082] Step S5 uses a nonlinear dynamic programming algorithm to solve the objective function and obtain the optimal path, specifically including:

[0083] S51. Based on the objective function obtained from S43, S44 and S45, use the Matlab nonlinear dynamic programming optimization toolbox to solve for the angular velocity at time t. Then, make the ASVs move to the turning point at the next time step according to the angular velocity. Repeat this process to obtain the optimal path planning scheme of the ASVs within a specified time.

[0084] Compared with the prior art, the present invention has the following advantages: the method is suitable for solving the problem of the optimal path planning problem of surface vehicles with the minimum positioning error when the prior information of the target position is unknown and there is nonlinear non-Gaussian noise. Attached Figure Description

[0085] Figure 1 Flowchart of an optimal path planning method for an autonomous surface vehicle with minimal target positioning error

[0086] Figure 2 This is a schematic diagram illustrating the impact of outliers on different pollutants under uniform distribution.

[0087] Figure 3 This is a schematic diagram of the rasterized uncertain region of the present invention.

[0088] Figure 4 A schematic diagram of the geometric relationship of ASVs relative to the target.

[0089] Figure 5 Pseudocode for the optimal path acquisition algorithm for localization of one ASV (Active Site Void) target in this invention

[0090] Figure 6 Pseudocode for the optimal path acquisition algorithm for localization of two ASVs in this invention

[0091] Figure 7 The pseudocode for the positioning algorithm of this invention is as follows.

[0092] Figure 8 This is a flowchart illustrating the optimal path acquisition process for the localization of three ASVs targets according to the present invention.

[0093] Figure 9 The experimental parameters for this invention are for a single ASVs scenario.

[0094] Figure 10 The experimental parameters for this invention are for two ASVs scenarios.

[0095] Figure 11 The experimental parameters for this invention are for three ASVs scenarios.

[0096] Figure 12 This is the optimal path diagram of an ASV under different pollutants according to the present invention.

[0097] Figure 13 This is the optimal path diagram for two ASVs when the pollution factor is 0.4 in this invention.

[0098] Figure 14 This is the optimal path (dynamic objective) for the three ASVs when the pollution factor is 0.4 in this invention.

[0099] Figure 15 This invention provides the three ASVs' ordinary S-shaped pathways (dynamic targets) when the pollution factor is 0.4.

[0100] Figure 16 This is a comparison chart of the positioning effects of the optimal path and the ordinary S-shaped path of the present invention. Detailed Implementation

[0101] The present invention will be further illustrated below with reference to the accompanying drawings by providing a detailed description of a preferred embodiment.

[0102] Figure 1 The present invention proposes a flowchart of an optimal path planning method for an autonomous surface vehicle with minimal target positioning error, which specifically includes:

[0103] S1. Construct a system model (motion model and observation model) for the autonomous surface vehicle, i.e., the system model includes a motion model and an observation model;

[0104] S2. The optimal geometric relationship between the autonomous surface vehicle and the target is derived.

[0105] S3. Obtain the target's prior location information through a gridded area method (for one or two autonomous surface vehicles) or positioning technology (for three autonomous surface vehicles);

[0106] S4. Utilize the Monte Carlo strategy to particleize nonlinear non-Gaussian ranging noise and construct a closed-loop expression for the objective function;

[0107] S5. Solve the objective function using a nonlinear dynamic programming algorithm to obtain the optimal path.

[0108] In this implementation example, step S1 specifically includes:

[0109] S11. Considering the constant water depth of the target, the 3D scene is mapped to 2D for resolution. Assuming the coordinate reference system is an East-North-Down geodetic coordinate system, the target's position at time t is... The position of the i-th ASVs at time t is Where T represents transpose. If ASVs can sense its azimuth angle φ(t), consider its input velocity v(t) and angular velocity r. i (t), then the state of the corresponding ASVs can be represented as

[0110]

[0111] Define two orthogonal unit vectors, i.e.

[0112]

[0113] The motion model of ASVs without considering flow velocity is as follows:

[0114] in, and φ represents the position of the i-th ASVs at the previous time and at the current time, respectively; i (t-1) and φ i (t) represents the azimuth information of the i-th ASVs at the previous time and at the current time, respectively; when the angular velocity is not 0, it indicates that the ASVs are moving in a curve during the corresponding time period, and conversely, when the angular velocity is 0, it means that the ASVs are moving in an approximately straight line.

[0115] S12 and ASVs are equipped with sensors capable of receiving signal strength. By acquiring their signal strength values, their positions relative to the target are calculated. The system's observation equations are further constructed using a path loss model.

[0116]

[0117] in, Pi represents the signal power value received by the i-th ASVs from the target at time t; P0 represents the power value transmitted by the target, i.e., assuming that the transmitted power value is equal at every time. Indicates the path loss factor; d represents the distance between the i-th ASV and the target at time t; d0 represents the reference distance, which is usually 1m; This indicates that the corresponding Gaussian noise is assumed to be equal at every moment. Its mean is zero and its variance is σ.

[0118] Due to the complex and variable marine environment, its noise is not only Gaussian noise but may also contain outliers, resulting in non-Gaussian noise characteristics. Introducing a noise pollution factor β, with a value range of [0,1), the probability distribution corresponding to non-Gaussian noise can be expressed as follows:

[0119] p(η)=(1-β)Ω(η;0,σ 2 )+βΦ(η), (5)

[0120] Wherein, Ω(η;0,σ 2 ) represents the Gaussian probability distribution; Φ(η) represents the probability distribution of non-Gaussian anomalous noise; Figure 2 The effect of different pollutant β on the intensity of the observed signal under uniform distribution.

[0121] Step S2 specifically includes:

[0122] S21. An optimal geometric relation exists if and only if the number of ASVs is greater than or equal to 2, such as Figure 4 As shown.

[0123] S22. For two ASVs scenarios, it is derived that the optimal geometric relationship is when the relative target angle is π / 2.

[0124] Corollary 1: Let α 12 Let α be the angle between ASVs 1 and ASVs 2, and let α be the angle between ASVs 1 and ASVs 2. 12 =α 21 For any fixed In other words, the angle that maximizes the determinant of the Fisher information matrix is ​​π / 2, i.e., α. 12 =α 21 =π / 2.

[0125] Proof 1: Let in And there exists a constant γ > 0 such that γ2 = ζγ1. At each time step, its corresponding function can be expressed as:

[0126]

[0127] To minimize the target positioning error, we need to maximize the determinant of equation (6), which is equivalent to minimizing equation (7), i.e.

[0128]

[0129] Through transformation, we can further obtain

[0130] γ1 2 [sin(2φ1(t))+ζsin(2φ2(t)) 2 +(cos(2φ1(t))+ζcos(2φ2(t)) 2 )]

[0131] =γ1 2 [1+ζ+ζcos(2φ1(t)-2φ2(t))]. (8)

[0132] It is obvious that when cos(2φ1(t)-2φ2(t))=-1, equation (8) can reach its minimum value. Therefore, for In other words, For two ASVs, the optimal geometric relationship can be achieved when their angle is π / 2, i.e. Q.E.D.

[0133] S23. For a scenario with three ASVs, the optimal geometric angles relative to the target are derived as follows: 1) Two ASVs have an angle of 0 or π relative to the target, and the angle of the third ASV relative to these two ASVs is π / 2; 2) All three ASVs have an angle of 2π / 3 relative to the target; 3) Two ASVs have an angle of π / 3 relative to the target, and the angle of the third ASV relative to the target is 2π / 3. The derivation process is as follows:

[0134] Let α 12 α 13 and α 23 This represents the angular relationship between the i-th ASVs and the l-th ASVs, where i, l ∈ {1, 2, 3}. γ il =γ i ·γ l And α 12 =α 21 α 13 =α 31 α23 =α 32 The angular relationship between the i-th ASVs and the l-th ASVs can be expressed as:

[0135]

[0136] If γ l >∑ 1≤i≤3,i≠l γ i This means that if an ASV is close to the target, then...

[0137]

[0138] Equation (10) can be further expressed as

[0139]

[0140] Here, \ represents the way the set is reduced.

[0141] When γ1 = γ2 = γ3, there exists a special optimal geometric relationship, namely α 12 =α 13 =α 23 =2π / 3 or α 12 =α 23 =π / 3, α 13 =2π / 3.

[0142] Step S3 specifically includes:

[0143] S31. When there is only 1 or 2 ASVs, quantify the uncertainty of the prior unknown of the target, such as Figure 3 The rasterized region is shown, with the intersections in the grid representing the possible locations of the target.

[0144] S32. When there are 3 ASVs, the prior location information of the target is obtained through positioning technology. If d0 = 1m, equation (4) in S12 can be obtained by transformation.

[0145]

[0146] Then, by expanding using a first-order Taylor series, we obtain...

[0147]

[0148] By performing a squared distance operation and combining it with weights, a nonlinear least squares expression is constructed, namely...

[0149]

[0150] in This indicates the corresponding estimated distance;

[0151] After expanding equation (14), it can be further transformed into a generalized trust domain subproblem, that is...

[0152]

[0153] Where y = [(B t ) T ,||B t || 2 ] T ;

[0154]

[0155] I and 0 represent the identity matrix and the zero matrix, respectively.

[0156] After obtaining the expression (15) for the generalized trust region subproblem, the prior location of the target is obtained by solving the bisection method. The pseudocode corresponding to the localization technique is as follows: Figure 7 As shown.

[0157] Step S4 utilizes a Monte Carlo strategy to particleize the nonlinear non-Gaussian ranging noise and constructs a closed-loop expression for the objective function, specifically including:

[0158] S41. To minimize the positioning estimation error, a Crommellow lower bound constructor from parameter estimation theory is introduced. The Fisher information matrix at time t is:

[0159]

[0160] Due to the presence of outliers, equation (17) cannot be expressed in a closed-loop manner. Therefore, by employing a Monte Carlo strategy to particleize the nonlinear non-Gaussian noise, equation (17) can be expressed as follows:

[0161]

[0162] in,

[0163]

[0164] ▽ η [p(η)] t The first-order gradient is represented by Equation (20), which is a particle-based operation using the Monte Carlo strategy to handle nonlinear non-Gaussian noise.

[0165] S42. For ASVs, to minimize the target localization error, it is necessary to maximize the determinant of the Fisher information matrix, i.e.

[0166]

[0167] in,

[0168] Taking the logarithm of equation (21), the objective function can be expressed as follows:

[0169]

[0170] As can be seen from equation (22), if angular velocity is taken as the variable, for the optimal path of ASVs, the parameter that needs to be solved at each moment is the corresponding angular velocity, i.e.

[0171]

[0172] in, represents the optimal angular velocity of the i-th ASVs within the time interval from 1 to M; c represents the corresponding constraint.

[0173] S43. When there is only one ASV, considering all the intersection points of the grid in the uncertain region as k, it is necessary to maximize the worst case in the uncertain region. Combining the set of all intersection points in the grid, according to the optimization equation of equation (23), the objective function for only one ASV can be expressed as follows:

[0174]

[0175] S44. When there are 2 ASVs, similarly considering all the intersection points of the grid in the uncertain region as k, it is necessary to maximize the worst case in the uncertain region. Combining the set of all intersection points in the grid, according to the optimization equation of equation (23), the objective function of 2 ASVs can be expressed as follows:

[0176]

[0177] S45. When there are 3 ASVs, the corresponding objective function is:

[0178]

[0179] Step S5 uses a nonlinear dynamic programming algorithm to solve the objective function and obtain the optimal path, specifically including:

[0180] S51. Based on the objective function obtained from S43, S44, and S45, the angular velocity at time t is solved using the Matlab nonlinear dynamic programming optimization toolbox. Then, the ASVs are moved to the turning point at the next time step according to this angular velocity. This process is repeated to obtain the optimal path planning scheme for the ASVs within a specified time. The pseudocode for the optimal path planning scheme that minimizes the target positioning error for one ASV is as follows: Figure 5 As shown; the pseudocode for the optimal path planning that minimizes the localization error for two ASVs is as follows: Figure 6As shown; the optimal path planning process for minimizing the localization error of the three ASVs is as follows: Figure 8 As shown.

[0181] To visualize the path planning method provided by this invention, simulations were performed in Matlab R2021b with the following parameter settings: P0 = -55dBm. σ = 2dB, I = 1000, r ∈ [-20°, 20°] rad / s. Additionally, the particle-based nonlinear non-Gaussian noise Monte Carlo strategy is executed 100 times, with N particles. C =1000.

[0182] When performing simulations in a scenario with only one ASV, the parameter settings are as follows: Figure 9 As shown in the figure. Under this parameter, the optimal target location path for different pollution factor conditions is as follows. Figure 12 As shown. From Figure 12 As can be seen, the larger the pollution factor, the larger the trajectory radius of the optimal path. The corresponding path will gradually approach the target. This is because the observation model calculates the distance between the target and ASVs based on signal strength. The closer to the target, the stronger the signal strength and the smaller the signal loss, which is beneficial for minimizing positioning errors.

[0183] Simulations were performed on two ASVs scenarios, with relevant parameter settings as follows: Figure 10 As shown in the diagram. Experiments with only one ASV scenario show that a larger contamination factor results in a larger trajectory radius. Therefore, in a two-ASV scenario, the median was taken, i.e., when the contamination factor was set to 0.4, to visualize the trajectory of the optimal path for the two ASVs, as shown below. Figure 13 As shown. From Figure 13 It can be seen that the optimal paths of the two ASVs gradually approach the uncertainty region of the target over time.

[0184] To explore the optimal path in three ASV scenarios, simulations were performed, and the relevant parameter settings are as follows: Figure 11 As shown in the diagram. Unlike the previous two scenarios, to better illustrate the effectiveness of the optimal path provided by this invention, the target location is dynamically changing. The relevant optimal path diagram is shown below. Figure 14 As shown, the path exhibits a curved movement trend around the target. To better compare the optimal positioning accuracy under the optimal path, a comparison is made with the currently more common S-shaped ordinary path scenario (such as...). Figure 15 (As shown). The error at each moment is calculated using positioning technology, such as... Figure 16 As shown, from Figure 16 It can be seen that, under the optimal path proposed in this invention, the positioning error is significantly better than the more common S-shaped path strategy at present.

[0185] Although the present invention has been described in detail through the preferred embodiments above, it should be understood that the above description should not be considered as a limitation of the present invention. Various modifications and substitutions to the present invention will be apparent to those skilled in the art after reading the above description. Therefore, the scope of protection of the present invention should be defined by the appended claims.

Claims

1. A method for optimal path planning of an autonomous surface vehicle with minimal target positioning error, characterized in that, Includes the following steps: S1. Construct a system model for an autonomous surface vehicle that includes a motion model and an observation model; S2. The optimal geometric relationship between the autonomous surface vehicle and the target is derived. S3. For one or two autonomous surface vehicles, the prior position information of the target is obtained through a gridded area method, or for three autonomous surface vehicles, the prior position information is obtained through positioning technology. S4. Utilize the Monte Carlo strategy to particleize the nonlinear non-Gaussian ranging noise and construct a closed-loop expression for the objective function, where: S41. To minimize the positioning estimation error, the Clummerroic lower bound constructor from parameter estimation theory is introduced. The Fisher information matrix at time t is: Among them, B t Indicates the position of the target at time t; This represents the position of the i-th autonomous surface vehicle (ASVs) at time t; φ represents the signal power received by the i-th ASVs from the target at time t; i (t) represents the azimuth information of the i-th ASVs at this moment; This represents the distance between the i-th ASV and the target at time t; Indicates the path loss factor; Using the Monte Carlo strategy, the nonlinear non-Gaussian noise is particleized, and equation (1) becomes: in, This represents the corresponding first-order gradient; Equation (4) is the operation of particleizing nonlinear non-Gaussian noise using the Monte Carlo strategy; Introducing a noise pollution factor β, whose value range is [0,1), the probability distribution p(η) corresponding to non-Gaussian noise can be expressed as: p(η)=(1-β)Ω(η;0,σ 2 )+βΦ(η) (5)where,Ω(η;0,σ 2 ) represents the Gaussian probability distribution; Φ(η) represents the probability distribution of non-Gaussian anomalous noise; S42. For ASVs, to minimize the target localization error, the determinant of the Fisher information matrix is ​​maximized, i.e.: in, Taking the logarithm of equation (6), the objective function becomes: With angular velocity as the variable, for the optimal path of ASVs, the parameter that needs to be solved at each moment is the corresponding angular velocity, that is: Where, r i * (t=1:M) represents the optimal angular velocity of the i-th ASVs within the time interval from 1 to M; c represents the corresponding constraint; S43. When there is only one ASV, considering all the intersections of the grid in the uncertain region as k, to maximize the worst case in the uncertain region, combined with the set of all intersections in the grid, according to the optimization equation of equation (8), the objective function for only one ASV is: S44. When there are 2 ASVs, considering all the intersection points of the grid in the uncertain region as k, to maximize the worst case in the uncertain region, combined with the set of all intersection points in the grid, according to the optimization equation of equation (8), the objective function of the 2 ASVs is: S45. When there are 3 ASVs, the corresponding objective function is: S5. Solve the objective function using a nonlinear dynamic programming algorithm to obtain the optimal path.

2. The optimal path planning method for an autonomous surface vehicle with minimum target positioning error as described in claim 1, characterized in that, Step S1, which involves constructing a system model for an autonomous surface vehicle that includes a motion model and an observation model, specifically includes: S11. Considering the known water depth of the target, the 3D scene is mapped to 2D for resolution. Assume the coordinate reference system is an East-North-Down geodetic coordinate system, and the target's position at time t is... The position of the i-th autonomous surface vehicle (ASVs) at time t is Where T represents transpose; if ASVs can sense its azimuth angle φ(t), consider its input velocity v(t) and angular velocity r. i (t), then the state of the corresponding ASVs can be represented as: Define two orthogonal unit vectors, i.e. The motion model of ASVs without considering flow velocity is as follows: in, and φ represents the position of the i-th ASVs at the previous time and at the current time, respectively; i (t-1) and φ i (t) represents the azimuth information of the i-th ASVs at the previous time and at the current time, respectively; when the angular velocity is not 0, it indicates that the ASVs are moving in a curve during the corresponding time period, and conversely, when the angular velocity is 0, it means that the ASVs are moving in an approximately straight line. S12 and ASVs are equipped with sensors capable of receiving signal strength. By acquiring their signal strength values, their positions relative to the target are calculated. A path loss model is then used to further construct the system's observation model, namely: in, Pi represents the signal power value received by the i-th ASVs from the target at time t; P0 represents the power value transmitted by the target, i.e., assuming that the transmitted power value is equal at every time. Indicates the path loss factor; d represents the distance between the i-th ASV and the target at time t; d0 represents the reference distance; This indicates that the corresponding Gaussian noise is assumed to be equal at every moment. Its mean is zero and its variance is σ.

3. The optimal path planning method for an autonomous surface vehicle with minimum target positioning error as described in claim 1, characterized in that, Step S2, which derives the optimal geometric relationship between the autonomous surface vehicle and the target, specifically includes: S21. An optimal geometric relation exists if and only if the number of ASVs is greater than or equal to 2. S22. For two ASVs scenarios, it is derived that the optimal geometric relationship is when the relative target angle is π / 2. Corollary 1: Let α 12 Let α be the angle between ASVs 1 and ASVs 2, and let α be the angle between them. 12 =α 21 For any fixed In other words, the angle that maximizes the determinant of the Fisher information matrix is ​​π / 2, i.e., α. 12 =α 21 =π / 2; Proof 1: Let in Furthermore, there exists a constant γ > 0 such that γ2 = ζγ1, and at each time step, its corresponding function can be expressed as: To minimize the target positioning error, we need to maximize the determinant of equation (16), which is equivalent to minimizing equation (17), i.e.: Through transformation, we can further obtain: It is obvious that when cos(2φ1(t)-2φ2(t))=-1, equation (18) can reach its minimum value. Therefore, for In other words, For two ASVs, the optimal geometric relationship can be achieved when their angle is π / 2, i.e. S23. For a scenario with three ASVs, the optimal geometric angles relative to the target are derived as follows: 1) When the angles of two ASVs relative to the target are 0 or π, and the angle of the third ASV relative to these two ASVs is π / 2; 2) When the angles of all three ASVs relative to the target are 2π / 3; 3) When the angles of two ASVs relative to the target are π / 3, and the angle of the third ASV relative to the target is 2π / 3. The derivation process is as follows: Let α 12 α 13 and α 23 This represents the angular relationship between the i-th ASVs and the l-th ASVs, where i, l ∈ {1, 2, 3}. γ il =γ i ·γ l And α 12 =α 21 α 13 =α 31 α 23 =α 32 The angular relationship between the i-th ASVs and the l-th ASVs can be expressed as: If γ l >∑ 1≤i≤3,i≠l γ i This means that if an ASV is close to the target, then: Equation (20) can be further expressed as: Where \ indicates a way of representing a reduction in the set; When γ1 = γ2 = γ3, there exists a special optimal geometric relationship, namely α 12 =α 13 =α 23 =2π / 3 or α 12 =α 23 =π / 3, α 13 =2π / 3.

4. The optimal path planning method for an autonomous surface vehicle with minimum target positioning error as described in claim 1, characterized in that, Step S3 obtains prior position information of the target for one or two autonomous surface vehicles (ASWs) using a gridded area method, or for three ASWs using localization technology. Specifically, it includes: S31. When there is only 1 or 2 ASVs, quantify the uncertainty of the target's prior position, rasterize the relevant area, and use the intersection of the grid as the possible position of the target. S32. When there are 3 ASVs, the prior position of the target is obtained through positioning technology. If d0 = 1m, equation (15) in S12 can be transformed to obtain: Then, by expanding using a first-order Taylor series, we obtain: By performing a squared distance operation and combining it with weights, a nonlinear least squares expression is constructed, namely: in This indicates the corresponding estimated distance; After expanding equation (24), it can be further transformed into a generalized trust domain subproblem, namely: Where y = [(B t ) T ,||B t || 2 ] T ; I and 0 represent the identity matrix and the zero matrix, respectively. After obtaining the expression (25) of the generalized trust domain subproblem, the prior position of the target is obtained by solving the bisection method.

5. The optimal path planning method for an autonomous surface vehicle with minimum target positioning error as described in claim 1, characterized in that, Step S5 uses a nonlinear dynamic programming algorithm to solve the objective function and obtain the optimal path, specifically including: S51. Based on the objective function obtained from S43, S44 and S45, use the Matlab nonlinear dynamic programming optimization toolbox to solve for the angular velocity at time t. Then, make the ASVs move to the turning point at the next time step according to the angular velocity. Repeat this process to obtain the optimal path planning scheme of the ASVs within a specified time.

Images