An online prediction method for trajectory tracking success rate of modular unmanned surface vessels.

Abstract

This invention relates to an online prediction method for trajectory tracking success rate of modular unmanned surface vessels (USVs). The method includes the following steps: S1: Establishing a relative motion model between the USV and the desired trajectory point using stochastic differential equations; S2: Dividing the feasible state space of the USV into multiple sets, and rasterizing the spatial regions within each set; S3: Calculating the Markov transition probability of any grid within the feasible state space; S4: Recursively calculating the trajectory tracking success rate of the USV using backpropagation. This invention introduces a spatial correlation function to describe the influence of wind, wave, and current disturbances on the relative pose of the USV and the desired trajectory through a stochastic relative motion model. Simultaneously, it utilizes Markov stochastic approximation theory to predict the trajectory tracking success rate online, significantly improving the accuracy of trajectory tracking success rate prediction compared to existing technologies.

Description

Technical Field

[0001] This invention belongs to the field of decision planning technology for marine unmanned systems, and particularly relates to an online prediction method for trajectory tracking success rate of modular unmanned surface vessels. Background Technology

[0002] Unmanned surface vessels (USVs) are autonomous, maneuvering intelligent entities without crew involvement. They have a wide range of important applications in marine engineering, such as ocean monitoring, hydrological observation, and biological research, meeting the needs of various marine scientific, engineering, and military missions. By using USVs as modules to build reconfigurable floating platforms based on the autonomous assembly of multiple USV modules—that is, modular USVs—the application scenarios of floating platforms in marine engineering will be greatly expanded.

[0003] Trajectory tracking is a crucial step in the transformation, reconstruction, and overall motion processes of modular unmanned surface vessels (USVs), requiring each USV to accurately track its target trajectory in real time. However, in real-world applications, factors such as wind, waves, and currents at sea can cause modular USVs to deviate from their original trajectories or even encounter danger. Therefore, online prediction of trajectory tracking success rates based on actual conditions is of great importance.

[0004] In existing technologies, the methods for tracking unmanned surface vessels are usually based on distance and bearing deviations. However, this approach is too simplistic and does not take into account external disturbances, resulting in low prediction accuracy in complex sea conditions. Summary of the Invention

[0005] The purpose of this invention is to solve one of the above-mentioned technical problems and provide an online prediction method for trajectory tracking success rate of modular unmanned surface vessels.

[0006] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0007] An online prediction method for trajectory tracking success rate of modular unmanned surface vessels includes the following steps:

[0008] S1: Stochastic differential equations are used to describe the motion of the modular unmanned surface vessel and its desired trajectory, and then a relative motion model between the unmanned surface vessel and the desired trajectory point is established based on the motion formula of the modular unmanned surface vessel and its desired trajectory.

[0009] S2: Divide the feasible state space of the unmanned surface vessel into multiple non-overlapping sets; these sets include, but are not limited to, the target set. Feasible state set and feasible state boundary set ;Scale-adaptive rasterization is performed on the spatial regions within each set;

[0010] S3: Calculate the Markov transition probability of any grid in the feasible state space based on the relative motion model, and then obtain the Markov chain model;

[0011] S4: Based on the Markov transition probability and Markov chain model, the backpropagation method is used to recursively calculate the trajectory tracking success rate of the unmanned surface vessel (USV). When the trajectory tracking success rate of the USV at the current position is lower than a predetermined threshold, the expected trajectory of the USV is replanned.

[0012] In some embodiments of the present invention, step S1 specifically includes the following steps:

[0013] S11: The motion of the modular unmanned surface vessel and its desired trajectory is described by stochastic differential equations;

[0014] The motion formula for the modular unmanned surface vessel is expressed as:

[0015] ;

[0016] The motion formula for the desired trajectory is expressed as:

[0017] ;

[0018] in, and The state vector represents the current state. , , , These represent the lateral distance, longitudinal distance, and heading angle of the unmanned surface vessel, respectively, and the state vector. , , , These represent the lateral distance, longitudinal distance, and heading angle of the desired trajectory point, respectively. Indicates the speed of the unmanned surface vessel. , , , These represent the lateral velocity, longitudinal velocity, and bow velocity of the unmanned surface vessel, respectively. This represents the velocity of the desired trajectory point. , , , These represent the lateral velocity, longitudinal velocity, and heading velocity of the desired trajectory point, respectively. To represent the function of the disturbance of wind, waves and current on the unmanned surface vessel, ; Let be a function representing the disturbance of wind, waves, and current on the desired trajectory point. ; Represents a third-order matrix. This represents a diagonal matrix whose diagonal elements are related to time. The variance represents the random disturbance. , , , These represent the variances of random disturbances in the lateral, longitudinal, and bow directions, respectively. and Both are functions representing random perturbations;

[0019] S12: Establish a relative motion model between the unmanned surface vessel (USV) and the desired trajectory point based on the motion formulas of the modular USV and the desired trajectory.

[0020] ;

[0021] in, This represents the pose of the unmanned surface vessel relative to the desired trajectory point. , This represents the velocity of the unmanned surface vessel relative to the desired trajectory point. , This represents the difference between the random disturbance experienced by the unmanned surface vessel and the random disturbance at the desired trajectory point.

[0022] In some embodiments of the present invention, step S1 further includes the following steps:

[0023] S13: Based on the relative motion model between the unmanned surface vessel and the desired trajectory point, Defined as:

[0024] ;

[0025] but The covariance is expressed as:

[0026] ;

[0027] based on The covariance is obtained The expression is:

[0028] ;

[0029] in, Represents the spatial dependence function, and The expression is:

[0030] ;

[0031] in, Represents the location correlation coefficient. Represents the attitude-related function;

[0032] S14: Based on The expression rewrites the relative motion model between the unmanned surface vessel and the desired trajectory point as follows:

[0033] ;

[0034] in For standard Brownian motion, For drift term, This is a diffusion term.

[0035] In some embodiments of the present invention, the function representing random perturbation is defined as , The formula for calculating the covariance is:

[0036] ;

[0037] in, It is a third-order identity matrix. The pose correlation function is defined when the pose of the unmanned surface vessel (USV) matches that of the desired trajectory point. ,but When the pose of the unmanned surface vessel is inconsistent with the desired trajectory point, ,but .

[0038] In some embodiments of the present invention, during step S2, when performing scale-adaptive rasterization on the spatial regions within each set, the dimension of each grid in the x-axis direction is defined as:

[0039] ;

[0040] in, This represents the distance between the current grid and the target set on the x-axis. This represents the distance between the current grid on the x-axis and the feasible state boundary set. Indicates the number of grid cells on the x-axis;

[0041] The dimension of each grid in the y-axis direction is defined as follows:

[0042] ;

[0043] in, This represents the distance between the current grid and the target set on the y-axis. This represents the distance between the current grid on the y-axis and the feasible state boundary set. Indicates the number of grid cells on the y-axis;

[0044] In each grid The dimension in the axial direction is defined as:

[0045] ;

[0046] in, express The distance between the current grid and the target set on the axis. express The distance between the current grid on the axis and the feasible state boundary set. express The number of grid cells on the axis The axis direction is perpendicular to the x-axis and y-axis directions.

[0047] The grid size in each axial direction can be uniformly described as follows: ,in , For standard scale, This is the proportionality coefficient.

[0048] In some embodiments of the present invention, the method for calculating the Markov transition probability of any grid q in the feasible state space includes the following steps:

[0049] Determine the set that grid q currently belongs to;

[0050] If grid Then the formula for calculating the Markov transition probability of grid q is:

[0051] ;

[0052] like Then the formula for calculating the Markov transition probability from grid q to the adjacent grid is:

[0053] ;

[0054] in, Indicates adjacent grids, Indicates the distance from the current grid q to the grid The k-th step transition probability.

[0055] In some embodiments of the present invention, Defined as:

[0056] ;

[0057] in, , , All are intermediate variables. , , The calculation formulas are as follows:

[0058] ;

[0059] ;

[0060] ;

[0061] in, The time sampling interval is... As a predetermined positive number, The range of values ​​for is:

[0062] ;

[0063] in, For drift term, For diffusion term, n Represents the dimension of motion. This represents random disturbance error.

[0064] In some embodiments of the present invention, based on the relative motion model between the unmanned surface vessel and the desired trajectory point, Represented as:

[0065] ;

[0066] Represented as:

[0067] ;

[0068] in, Represents the location correlation coefficient. Represents the attitude-related function. , , express The positional deviation between the unmanned surface vessel and the desired trajectory point at any given time.

[0069] In some embodiments of the present invention, the specific method for recursively calculating the success rate of unmanned surface vessel trajectory tracking using the backpropagation method in step S4 is as follows:

[0070] By a predetermined time interval Internal from target set Backpropagation Markov transition probability calculation of the feasible state set at time t Each grid q in the future Time to reach the target set The probability of.

[0071] In some embodiments of the present invention, the specific method for recursively calculating the success rate of unmanned surface vessel trajectory tracking using the backpropagation method in step S4 includes the following steps:

[0072] Define the number of backpropagations ;

[0073] The initial success probability of each grid cell is set to:

[0074] ;

[0075] for The formula for calculating the success probability of each grid cell is:

[0076] ;

[0077] in, Let be the success probability of grid point q at step k. and These represent the steps from the grid at step k. To itself and adjacent grids Markov transition probabilities, and Representing grids respectively and adjacent grids The probability of success in the (k+1)th step;

[0078] The success probability of any grid point is recursively calculated based on the initial success probability value of each grid and the calculation formula for the success probability of each grid.

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

[0080] This invention establishes a relative motion stochastic model of a modular unmanned surface vessel (USV) and a desired trajectory, and introduces a spatial correlation function into this model to describe the influence of wind, wave, and current disturbances on their relative pose, laying the foundation for improving the accuracy of subsequent predictions. Simultaneously, it utilizes Markov random approximation theory to predict the trajectory tracking success rate online. Compared to existing methods that rely on pose deviations, this significantly improves the accuracy of predicting the USV's trajectory tracking success rate and demonstrates higher reliability.

[0081] Other features and advantages of the invention will be set forth in the description which follows, and will be apparent in part from the description, or may be learned by practicing the invention. The objects and other advantages of the invention may be realized and obtained by means of the structures pointed out in the description, claims and drawings. Attached Figure Description

[0082] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0083] Figure 1 This is a schematic diagram of a method for online prediction of trajectory tracking success rate for modular unmanned surface vessels.

[0084] Figure 2 This is a schematic diagram of the sets of average rasterization in the xy plane provided in an embodiment of the present invention;

[0085] Figure 3 A schematic diagram of a Markov chain provided for an embodiment of the present invention;

[0086] Figure 4 A schematic diagram of wind, wave, and current disturbances provided in an embodiment of the present invention;

[0087] Among them, (a) is a schematic diagram of wind, wave and current disturbance at t=0s, and (b) is a schematic diagram of wind, wave and current disturbance at t=6s;

[0088] Figure 5 A schematic diagram illustrating the trajectory tracking success rate prediction results in a undisturbed scenario provided by an embodiment of the present invention;

[0089] Among them, (a) is the tracking success probability map in the rasterized state space in the undisturbed scene, and (b) is the contour map of the tracking success probability in the undisturbed scene.

[0090] Figure 6 This is a schematic diagram illustrating the trajectory tracking success rate prediction results in a disturbed scenario provided by an embodiment of the present invention.

[0091] Among them, (a) is the tracking success probability map in the rasterized state space in a disturbed scene, and (b) is the contour map of the tracking success probability in a disturbed scene. Detailed Implementation

[0092] To make the objectives, technical solutions, and advantages of this application clearer, the application is described and illustrated below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application. All other embodiments obtained by those skilled in the art based on the embodiments provided in this application without inventive effort are within the scope of protection of this application.

[0093] It should be noted that the terminology used herein is for the purpose of describing particular implementations only and is not intended to limit the exemplary implementations according to this application. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. Furthermore, it should be understood that the terms “comprising” and “having”, and any variations thereof, are intended to cover non-exclusive inclusion, for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such process, method, product, or apparatus.

[0094] To better explain the solution of the present invention, the background technology of the present invention will be described first.

[0095] Floating platforms are used for offshore production, operations, and observation activities, playing a vital role in marine engineering. Compared to traditional fixed floating platforms, multi-module parallel floating platforms can be deformed and reconfigured according to mission requirements, offering greater autonomy and flexibility. However, existing modular floating platforms are mostly manually assembled, resulting in insufficient adaptability to various scenarios. Unmanned surface vessels (USVs) are autonomously moving intelligent agents. By using USVs as modules to build a reconfigurable floating platform based on the autonomous assembly of multiple USV modules (hereinafter referred to as modular USVs), the application scenarios of such platforms will be greatly expanded.

[0096] Trajectory tracking is a crucial step in the transformation, reconfiguration, and overall motion processes of modular unmanned surface vessels (USVs), requiring the platform to accurately track target points in real time. Considering that modular USVs are typical overacting systems (i.e., the number of actuators exceeds the degrees of freedom), their trajectory tracking task will simultaneously include tracking both position and attitude (i.e., heading angle) in the horizontal plane. However, influenced by factors such as wind, waves, and currents at sea, modular USVs may deviate from their original trajectory or even encounter danger. Therefore, online prediction of trajectory tracking success rate based on actual conditions is crucial. When the success rate falls below a threshold, it can be considered that the platform is unable to track the predetermined trajectory. In this case, trajectory replanning must be performed starting from the current position to ensure successful completion of the task even under external influences.

[0097] Traditional methods for assessing whether an unmanned surface vessel (USV) has successfully tracked a desired trajectory often rely directly on distance and orientation deviations. However, this approach is overly simplistic, neglecting external disturbances and lacking theoretical support. Success rate prediction methods consider the randomness and uncertainty inherent in the motion process, describing the problem as a probabilistic reachability problem for a target set within a trajectory. This can be solved by deriving an analytical solution to the target set's reachability power. Currently, research on trajectory tracking success rate prediction is lacking, although this problem is somewhat similar to the probability prediction problem for aerial refueling and docking. The trajectory tracking problem is a complex nonlinear process involving a large number of process variables. Therefore, various numerical methods have been proposed to solve the probabilistic reachability problem, mainly including three categories: model-based methods, limit approximation methods, and data-driven methods. Model-based methods require simplification and abstraction of the original complex process, such as discretizing the state space using a grid and generating a Markov chain as an approximation of the complex process, then calculating the probability of the system's current state reaching the target set. Limit approximation methods typically use compact forms such as polyhedra, hyperrectangles, elliptic bodies, or polygons to approximate the reachable set, obtaining an effective limit approximation. Data-driven methods train deep networks to extract useful feature information, directly predicting the docking success rate. Existing methods each have their advantages and disadvantages. For example, while model-based and limit approximation methods have low computational cost, they may be overly conservative. Data-driven methods, while capable of fitting complex processes, require a large amount of actual experimental data. Furthermore, there is currently limited research directly targeting modular unmanned surface vessels (USVs). Given the dynamic changes in the target set, the large state space, and the long forward prediction time in the trajectory tracking process, directly applying existing success rate prediction methods would lead to a series of problems, such as a lack of organic integration with actual control schemes and motion models, and overly dense prediction results.

[0098] Where there is no conflict, the embodiments and features in the embodiments of the present invention can be combined with each other.

[0099] The technical solution of the present invention will be described in detail below with reference to specific embodiments and accompanying drawings.

[0100] As attached Figure 1 -Appendix Figure 6 As shown, in an illustrative embodiment of an online prediction method for trajectory tracking success rate of a modular unmanned surface vessel according to the present invention, the prediction method includes the following steps.

[0101] S1: Stochastic differential equations are used to describe the motion of the modular unmanned surface vessel and its desired trajectory. Then, a relative motion model between the unmanned surface vessel and the desired trajectory point is established based on the motion formula of the modular unmanned surface vessel and its desired trajectory.

[0102] In some embodiments of the present invention, step S1 specifically includes the following steps.

[0103] S11: The motion of the modular unmanned surface vessel and its desired trajectory is described by stochastic differential equations.

[0104] Specifically, the motion formula for the modular unmanned surface vessel is expressed as:

[0105] .

[0106] The motion formula for the desired trajectory is expressed as:

[0107] .

[0108] in, and The state vector represents the current state. , , , These represent the lateral distance, longitudinal distance, and heading angle of the unmanned surface vessel, respectively, and the state vector. , , , These represent the lateral distance, longitudinal distance, and heading angle of the desired trajectory point, respectively. Indicates the speed of the unmanned surface vessel. , , , These represent the lateral velocity, longitudinal velocity, and bow velocity of the unmanned surface vessel, respectively. This represents the velocity of the desired trajectory point. , , , These represent the lateral velocity, longitudinal velocity, and heading velocity of the desired trajectory point, respectively. To represent the function of the disturbance of wind, waves and current on the unmanned surface vessel, ; Let be a function representing the disturbance of wind, waves, and current on the desired trajectory point. ; Represents a third-order matrix. This represents a diagonal matrix whose diagonal elements are related to time. The variance represents the random disturbance. , , , These represent the variances of random disturbances in the lateral, longitudinal, and bow directions, respectively. and Both are functions representing random perturbations.

[0109] It should be noted that this random perturbation is not a standard Brownian motion, but is represented as a change in state relative to the current state. and Related functions. In some embodiments of the present invention, the function representing random perturbation is defined as... , The formula for calculating the covariance is:

[0110] ;

[0111] in, It is a third-order identity matrix. The pose correlation function is defined when the pose of the unmanned surface vessel (USV) matches that of the desired trajectory point. ,but When the pose of the unmanned surface vessel is inconsistent with the desired trajectory point, ,but .

[0112] S12: Subtract the motion formula of the modular unmanned surface vessel from the motion formula of the desired trajectory to obtain the relative motion model between the unmanned surface vessel and the point on the desired trajectory:

[0113] .

[0114] in, This represents the pose of the unmanned surface vessel relative to the desired trajectory point. , This represents the velocity of the unmanned surface vessel relative to the desired trajectory point. , This represents the difference between the random disturbance experienced by the unmanned surface vessel and the random disturbance at the desired trajectory point.

[0115] In some embodiments of the present invention, step S1 further includes the following steps.

[0116] S13: Based on the relative motion model between the unmanned surface vessel and the desired trajectory point, Defined as:

[0117] .

[0118] but The covariance is expressed as:

[0119] .

[0120] based on The covariance is obtained The expression is:

[0121] .

[0122] in, Represents the spatial dependence function, and The expression is:

[0123] .

[0124] in, Represents the location correlation coefficient. The attitude correlation function represents the degree of correlation between the position correlation coefficient and the attitude correlation function, reflecting the relative pose between the unmanned surface vessel and the trajectory point and the disturbances of wind, waves and current.

[0125] S14: Based on The expression rewrites the relative motion model between the unmanned surface vessel and the desired trajectory point as follows:

[0126] .

[0127] in For standard Brownian motion, For drift term, This is a diffusion term.

[0128] S2: Divide the feasible state space of the unmanned surface vessel into multiple non-overlapping sets. These sets include, but are not limited to, the target set. Feasible state set and feasible state boundary set .

[0129] Specifically, the desired state of the unmanned surface vessel at time t is defined as follows: The desired trajectory point at time t is defined as the target set, and the region surrounding that trajectory point is defined as the target set. Based on task constraints, other state spaces can be divided into a set of deterministic feasible states. Feasible state boundary set The size of the area surrounding the trajectory point depends on the maximum allowable value of the tracking error and needs to be preset.

[0130] Each spatial region within a set is subjected to scale-adaptive rasterization.

[0131] Specifically, for regions far from the target set or feasible state boundary set, sparse Markov chains can be constructed to achieve fast computation, resulting in a sparser mesh, meaning a larger mesh scale for each degree of freedom. Conversely, for regions closer to the target set or feasible state boundary set, denser Markov chains are needed to achieve accurate computation, resulting in a denser mesh, meaning a smaller mesh scale.

[0132] In some embodiments of the present invention, during step S2, when performing scale-adaptive rasterization on the spatial regions within each set, the dimension of each grid in the x-axis direction is defined as:

[0133] ;

[0134] in, This represents the distance between the current grid and the target set on the x-axis. This represents the distance between the current grid on the x-axis and the feasible state boundary set. Indicates the number of grid cells on the x-axis;

[0135] The dimension of each grid in the y-axis direction is defined as follows:

[0136] ;

[0137] in, This represents the distance between the current grid and the target set on the y-axis. This represents the distance between the current grid on the y-axis and the feasible state boundary set. Indicates the number of grid cells on the y-axis;

[0138] In each grid The dimension in the axial direction is defined as:

[0139] ;

[0140] in, express The distance between the current grid and the target set on the axis. express The distance between the current grid on the axis and the feasible state boundary set. express The number of grid cells on the axis The axial direction is perpendicular to both the x-axis and y-axis. Subsequently, the mesh size in each axial direction can be uniformly described as... ,in , For standard scale, This is the proportionality coefficient.

[0141] The above grid size strategy can simultaneously achieve both computational efficiency and accuracy.

[0142] In a specific embodiment of the present invention, a schematic diagram of each set of average rasterization in the xy plane is shown below. Figure 2 As shown. Since trajectory tracking tasks require the relative pose of the unmanned surface vessel (USV) to be as close to zero as possible to the desired trajectory, in the target set... The longitudinal / lateral distance and heading angle of the unmanned surface vessel relative to the desired trajectory point can be used as relevant states. Given a target set... The relative state of the modular unmanned surface vessel at time t is Then predict The success rate of trajectory tracking at any given time can be expressed as: .

[0143] S3: Calculate the Markov transition probability of any grid in the feasible state space based on the relative motion model, and then obtain the Markov chain model.

[0144] Based on the gridded spatial region established in step S2, a Markov chain is constructed to describe the state transition model of the unmanned surface vessel. Specifically, such as... Figure 3 As shown, for a certain grid q You can to The direction of the adjacent grid and its own transfer, its adjacent grid set include , , , , , ,in , , With diagonal matrix elements in Related, expressed as and .

[0145] Based on the above definition, in some embodiments of the present invention, the method for calculating the Markov transition probability of any grid q in the feasible state space includes the following steps.

[0146] Determine the set that grid q is currently in.

[0147] If grid Then the formula for calculating the Markov transition probability of grid q is:

[0148] .

[0149] This calculation formula explains that a grid q belonging to the feasible state boundary set will not transfer to other grids because this set is an absorbing region.

[0150] like Then the formula for calculating the Markov transition probability from grid q to the adjacent grid is:

[0151] .

[0152] in, Indicates adjacent grids, Indicates the distance from the current grid q to the grid The k-th step transition probability.

[0153] In some embodiments of the present invention, Defined as:

[0154] .

[0155] in, , , All are intermediate variables, and , , The calculation formulas are as follows:

[0156] ;

[0157] ;

[0158] .

[0159] in, The time sampling interval is... As a predetermined positive number, The range of values ​​for is:

[0160] .

[0161] in, For drift term, For diffusion term, n Represents the dimension of motion. This represents random disturbance error.

[0162] In some embodiments of the present invention, based on the relative motion model between the unmanned surface vessel and the desired trajectory point, Represented as:

[0163] .

[0164] Represented as:

[0165] .

[0166] in, Represents the location correlation coefficient. Represents the attitude-related function. , , express The positional deviation between the unmanned surface vessel and the desired trajectory point at any given time.

[0167] The smaller the time sampling interval, the higher the accuracy of the aforementioned transition probability calculation and relative motion model, but the computational cost increases accordingly. When the grid size approaches 0, the Markov chain can weakly converge to the solution of the relative motion model between the unmanned surface vessel and the desired trajectory point.

[0168] S4: Based on the Markov transition probability and Markov chain model, the backpropagation method is used to recursively calculate the trajectory tracking success rate of the unmanned surface vessel (USV). When the trajectory tracking success rate of the USV at the current position is lower than a predetermined threshold, the expected trajectory of the USV is replanned.

[0169] In some embodiments of the present invention, the specific method for recursively calculating the success rate of unmanned surface vessel trajectory tracking using the backpropagation method in step S4 is as follows:

[0170] Based on the Markov chain and transition probability calculation formula established in step S3, we use the backpropagation method to predict the success rate of unmanned surface vessel trajectory tracking. That is, by means of a predetermined time interval Internal from target set Backpropagation Markov transition probability calculation of the feasible state set at time t Each grid q in the future Time to reach the target set The probability of.

[0171] In some embodiments of the present invention, the specific method for recursively calculating the success rate of unmanned surface vessel trajectory tracking using the backpropagation method in step S4 includes the following steps.

[0172] Define the number of backpropagations .

[0173] The initial success probability of each grid cell is set to:

[0174] .

[0175] for The formula for calculating the success probability of each grid cell is:

[0176] .

[0177] in, Let be the success probability of grid point q at step k. and These represent the steps from the grid at step k. To itself and adjacent grids Markov transition probabilities, and Representing grids respectively and adjacent grids The probability of success at step (k+1). Target set. For an absorption region, when At that time, the success rate of the grid points is 1. Correspondingly, the feasible state boundary set... It is also an absorption region, when At that time, the success rate of grid points was 0.

[0178] The success probability of any grid point is recursively calculated based on the initial success probability value of each grid and the calculation formula for the success probability of each grid.

[0179] Using the above calculation method, the trajectory tracking success rate of the unmanned surface vessel (USV) at the current grid point can be determined. When this value is lower than the safety threshold, the USV needs to perform online trajectory replanning, that is, abandon the current trajectory and have the planner replan a new trajectory starting from the current point, and then track the new trajectory.

[0180] In a specific embodiment of the present invention, comparative experiments were conducted in both undisturbed and disturbed scenarios to verify the effectiveness of the prediction method provided by the present invention.

[0181] If the effects of wind, waves, and current disturbances are not considered, then it means .

[0182] If the effects of wind, waves, and current disturbances are considered, the disturbance function The specific form is:

[0183] ,

[0184] It can be viewed as a clockwise rotating storm with its center shifting over time, such as Figure 4 As shown, (a) is a schematic diagram of wind, wave and current disturbance at t=0s, and (b) is a schematic diagram of wind, wave and current disturbance at t=6s.

[0185] Taking a desired trajectory point at a certain moment as an example, and defining the two surrounding grids as the target set, the success rate calculation results of the unmanned surface vessel tracking this trajectory point at different locations (i.e., the full state space) under undisturbed and disturbed conditions are as follows: Figure 5 and 6 As shown, the experimental results are divided into two parts, among which... Figure 5 (a) and Figure 6 (a) is the tracking success probability map in the rasterized state space. Figure 5 (b) and Figure 6 (b) is a contour plot of the tracking success probability. In the contour plot, the square at the origin represents the target set, and its outer envelope represents the probability contour lines. State grids on the same contour line have equal tracking success rates. (Comparison) Figure 5 and Figure 6 As can be seen, since the disturbances of wind, waves and currents tend to be clockwise, the contour map of the tracking success probability under disturbances will bend counterclockwise compared to the result under undisturbed conditions.

[0186] Finally, it should be noted that the various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. The same or similar parts between the various embodiments can be referred to each other.

[0187] The above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them; although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications can still be made to the specific implementation of the present invention or equivalent substitutions can be made to some technical features without departing from the spirit of the technical solutions of the present invention, and all such modifications and substitutions should be covered within the scope of the technical solutions claimed in the present invention.

Claims

1. A method for online prediction of trajectory tracking success rate for modular unmanned surface vessels, characterized in that, Includes the following steps: S1: Stochastic differential equations are used to describe the motion of the modular unmanned surface vessel and its desired trajectory, and then a relative motion model between the unmanned surface vessel and the desired trajectory point is established based on the motion formula of the modular unmanned surface vessel and its desired trajectory. Step S1 specifically includes the following steps: S11: The motion of the modular unmanned surface vessel and its desired trajectory is described by stochastic differential equations; The motion formula for the modular unmanned surface vessel is expressed as: ； The motion formula for the desired trajectory is expressed as: ； in, and The state vector represents the current state. , , , These represent the lateral distance, longitudinal distance, and heading angle of the unmanned surface vessel, respectively, and the state vector. , , , These represent the lateral distance, longitudinal distance, and heading angle of the desired trajectory point, respectively. Indicates the speed of the unmanned surface vessel. , , , These represent the lateral velocity, longitudinal velocity, and bow velocity of the unmanned surface vessel, respectively. This represents the velocity of the desired trajectory point. , , , These represent the lateral velocity, longitudinal velocity, and heading velocity of the desired trajectory point, respectively. To represent the function of the disturbance of wind, waves and current on the unmanned surface vessel, ; Let be a function representing the disturbance of wind, waves, and current on the desired trajectory point. ; This represents a diagonal matrix whose diagonal elements are related to time. The variance represents the random disturbance. , , , These represent the variances of random disturbances in the lateral, longitudinal, and bow directions, respectively. and Both are functions representing random perturbations; S12: Based on the motion formula of the modular unmanned surface vessel and the motion formula of the desired trajectory, establish a relative motion model between the unmanned surface vessel and the desired trajectory point: ； in, This represents the pose of the unmanned surface vessel relative to the desired trajectory point. , This represents the velocity of the unmanned surface vessel relative to the desired trajectory point. , This represents the difference between the random disturbance experienced by the unmanned surface vessel and the random disturbance at the desired trajectory point; S13: Based on the relative motion model between the unmanned surface vessel and the desired trajectory point, Defined as: ； but The covariance is expressed as: ； Based on the above The covariance is obtained The expression is: ； in, Represents the spatial dependence function, and The expression is: ； in, Represents the location correlation coefficient. Represents the attitude-related function; S14: Based on the above The expression rewrites the relative motion model between the unmanned surface vessel and the desired trajectory point as follows: ； in For standard Brownian motion, For drift term, For diffusion terms; S2: Divide the feasible state space of the unmanned surface vessel into multiple non-overlapping sets; the multiple sets include, but are not limited to, the target set. Feasible state set and feasible state boundary set ;Scale-adaptive rasterization is performed on the spatial regions within each set; S3: Calculate the Markov transition probability of any grid in the feasible state space based on the relative motion model; S4: Based on the Markov transition probability, the backpropagation method is used to recursively calculate the unmanned surface vessel trajectory tracking success rate.

2. The online prediction method for trajectory tracking success rate of modular unmanned surface vessels according to claim 1, characterized in that, The function representing random perturbation is defined as , The formula for calculating the covariance is: ； in, It is a third-order identity matrix. The pose correlation function is defined when the pose of the unmanned surface vessel (USV) matches that of the desired trajectory point. ,but When the pose of the unmanned surface vessel is inconsistent with the desired trajectory point, ,but .

3. The online prediction method for trajectory tracking success rate of modular unmanned surface vessels according to claim 1 or 2, characterized in that, In step S2, during the scale-adaptive rasterization process of the spatial regions within each set, the dimension of each grid in the x-axis direction is defined as follows: ； in, This represents the distance between the current grid and the target set on the x-axis. This represents the distance between the current grid on the x-axis and the feasible state boundary set. Indicates the number of grid cells on the x-axis; The dimension of each grid in the y-axis direction is defined as follows: ； in, This represents the distance between the current grid and the target set on the y-axis. This represents the distance between the current grid on the y-axis and the feasible state boundary set. Indicates the number of grid cells on the y-axis; In each grid The dimension in the axial direction is defined as: ； in, express The distance between the current grid and the target set on the axis. express The distance between the current grid on the axis and the feasible state boundary set. express The number of grids on the axis, the The axial direction is perpendicular to both the x-axis and the y-axis. The grid size in each axial direction is defined as follows: ; in , For standard scale, This is the proportionality coefficient.

4. The online prediction method for trajectory tracking success rate of modular unmanned surface vessels according to claim 3, characterized in that, The method for calculating the Markov transition probability of any grid q in the feasible state space includes the following steps: Determine the set that grid q currently belongs to; If grid Then the formula for calculating the Markov transition probability of grid q is: ； like Then the formula for calculating the Markov transition probability from grid q to the adjacent grid is: ； in, Indicates adjacent grids, Indicates the distance from the current grid q to the grid The k-th step transition probability.

5. The online prediction method for trajectory tracking success rate of modular unmanned surface vessels according to claim 4, characterized in that, The Defined as: ； in, , , All are intermediate variables. , , The calculation formulas are as follows: ； ； ； in, The time sampling interval is... As a predetermined positive number, The range of values ​​for is: ； in, For drift term, For diffusion term, n Represents the dimension of motion. This represents random disturbance error.

6. The online prediction method for trajectory tracking success rate of modular unmanned surface vessels according to claim 5, characterized in that, Based on the relative motion model between the unmanned surface vessel and the desired trajectory point, the Represented as: ； The Represented as: ； in, Represents the location correlation coefficient. Represents the attitude-related function. , , express The positional deviation between the unmanned surface vessel and the desired trajectory point at any given time.

7. The online prediction method for trajectory tracking success rate of modular unmanned surface vessels according to claim 1 or 6, characterized in that, The specific method for recursively calculating the success rate of unmanned surface vessel trajectory tracking using backpropagation in step S4 is as follows: By a predetermined time interval Internal from target set Backpropagation Markov transition probability calculation of the feasible state set at time t Each grid q in the future Time to reach the target set The probability of.

8. The online prediction method for trajectory tracking success rate of modular unmanned surface vessels according to claim 7, characterized in that, The specific method for recursively calculating the success rate of unmanned surface vessel trajectory tracking using the backpropagation method in step S4 includes the following steps: Define the number of backpropagations ; The initial success probability of each grid cell is set to: ； for The formula for calculating the success probability of each grid cell is: ； in, Let be the success probability of grid point q at step k. and These represent the steps from the grid at step k. To itself and adjacent grids Markov transition probabilities, and Representing grids respectively and adjacent grids The probability of success in the (k+1)th step; The success probability of any grid point is recursively calculated based on the initial success probability value of each grid and the calculation formula for the success probability of each grid.

Images