Trajectory tracking success rate online prediction method for modular unmanned ship

Abstract

The invention relates to a trajectory tracking success rate online prediction method for a modular unmanned ship. The method comprises the following steps: S1, establishing a relative motion model of the unmanned ship and an expected trajectory point by adopting a stochastic differential equation; s2, dividing the feasible state space of the unmanned ship into a plurality of sets, and performing rasterization processing on the space region in each set; s3, calculating the Markov transition probability of any grid in the feasible state space; and S4, recursively calculating the trajectory tracking success rate of the unmanned ship by adopting a back propagation method. According to the method, a space correlation function is introduced through a relative motion random model of the unmanned ship and an expected trajectory to describe the influence of wind wave flow disturbance on the relative poses of the unmanned ship and the expected trajectory, and meanwhile, the trajectory tracking success rate is predicted online by using the Markov random approximation theory; compared with the prior art, the accuracy of predicting the trajectory tracking success rate of the unmanned ship is remarkably improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of decision-making and planning of marine unmanned systems, and in particular relates to an online prediction method for the trajectory tracking success rate of a modular unmanned boat. Background Art

[0002] Unmanned vehicles (UDVs) are autonomous, unmanned vehicles with the ability to navigate autonomously. They have broad and important applications in marine engineering, such as ocean monitoring, hydrological observation, and biological research, meeting the needs of various marine science, engineering, and military missions. By using UDVs as modules and constructing reconfigurable floating platforms based on the autonomous assembly of multiple UDV modules—known as modular UDVs—they will significantly expand the application of floating platforms in marine engineering.

[0003] Trajectory tracking is a key step in the deformation, reconstruction, and overall motion of modular unmanned vehicles (UAVs). It requires each UAV to accurately track its target point in real time. However, in real-world applications, when affected by factors such as wind, waves, and currents at sea, modular UAVs can deviate from their original trajectory, even endangering their lives. Therefore, online prediction of trajectory tracking success rates based on actual conditions is crucial.

[0004] In the existing technology, the method of tracking the trajectory of unmanned boats is usually to directly judge based on distance deviation and azimuth deviation. However, this method is too simple and does not take into account the effect of external disturbances. In complex sea conditions, the prediction accuracy is low. Summary of the Invention

[0005] The purpose of the present invention is to solve one of the above technical problems and provide an online prediction method for the trajectory tracking success rate of a modular unmanned boat.

[0006] To achieve the above object, the technical solution adopted by the present invention is: An online prediction method for trajectory tracking success rate of a modular unmanned watercraft comprises the following steps: S1: Use stochastic differential equations to describe the motion of the modular unmanned boat and its desired trajectory, and then establish a relative motion model between the unmanned boat and the desired trajectory points based on the motion formula of the modular unmanned boat and its desired trajectory; S2: Divide the feasible state space of the unmanned boat into multiple non-overlapping sets; the multiple sets include but are not limited to the target set , feasible state set and the feasible state boundary set ; Perform scale-adaptive rasterization 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, and then obtain the Markov chain model; S4: Based on the Markov transition probability and Markov chain model, the back propagation method is used to recursively calculate the trajectory tracking success rate of the unmanned boat; when the trajectory tracking success rate of the unmanned boat at the current position is lower than the predetermined threshold, the desired trajectory of the unmanned boat is replanned.

[0007] In some embodiments of the present invention, step S1 specifically includes the following steps: S11: Using stochastic differential equations to describe the motion of modular unmanned vehicles and their desired trajectories; The motion formula of the modular unmanned boat is expressed as: ; The motion formula of the desired trajectory is expressed as: ; in, and is the state vector representing the current state, the state vector , 、 、 Represent the lateral distance, longitudinal distance and heading angle of the unmanned boat respectively, and the state vector , 、 、 They represent the lateral distance, longitudinal distance and heading angle of the desired track point respectively; Indicates the speed of the unmanned boat. , 、 、 Respectively represent the lateral speed, longitudinal speed and bow speed of the unmanned boat; represents the movement speed of the desired trajectory point, , 、 、 They represent the lateral velocity, longitudinal velocity and heading velocity of the desired trajectory point respectively; is a function that represents the disturbance of wind, waves and currents on the unmanned boat, ; is a function that represents the disturbance of wind, wave and current on the desired trajectory point, ; represents a third-order matrix, represents a diagonal matrix whose diagonal elements are time-dependent; represents the variance of the random disturbance, , 、 、 denote the random disturbance variances in the transverse, longitudinal and heading directions respectively; and Both are functions representing random disturbances; S12: Based on the motion formula of the modular unmanned boat and the motion formula of the desired trajectory, a relative motion model between the unmanned boat and the desired trajectory point is established: ; in, represents the position of the unmanned boat relative to the desired trajectory point, , represents the speed of the unmanned boat relative to the desired trajectory point, , It represents the difference between the random disturbance on the unmanned boat and the random disturbance at the expected trajectory point.

[0008] In some embodiments of the present invention, step S1 further includes the following steps: S13: Based on the relative motion model between the unmanned boat and the desired trajectory point, Defined as: ; but The covariance of is expressed as: ; based on The covariance of The expression is: ; in, represents the spatial correlation function, and The expression is: ; in, represents the position correlation coefficient, represents the posture-related function; S14: Based on The relative motion model between the unmanned boat and the desired trajectory point is re-described as: ; in is the standard Brownian motion, is the drift term, is the diffusion term.

[0009] In some embodiments of the present invention, the function representing random disturbance is defined as , The formula for calculating the covariance of is: ; in, is the third-order identity matrix, is the posture correlation function. When the posture of the unmanned boat is consistent with the expected trajectory point, ,but ; When the position of the unmanned boat is inconsistent with the expected trajectory point, ,but .

[0010] In some embodiments of the present invention, in step S2, during the scale-adaptive rasterization process of the spatial regions within each set, the size of each grid in the x-axis direction is defined as: ; in, Indicates the distance between the current grid and the target set on the x-axis, Indicates the distance between the current grid and the feasible state boundary set on the x-axis, Indicates the number of grids on the x-axis; The size of each grid in the y-axis direction is defined as: ; in, Indicates the distance between the current grid and the target set on the y-axis, Indicates the distance between the current grid and the feasible state boundary set on the y-axis, Indicates the number of grids on the y-axis; Each grid The dimensions in the axial direction are defined as: ; in, express The distance between the current grid and the target set on the axis, express The distance between the current grid and the feasible state boundary set on the axis, express The number of grids on the axis, The axis direction is perpendicular to the x-axis direction and the y-axis direction.

[0011] The grid size in each axis direction can be uniformly described as ,in , For standard scale, is the proportional coefficient.

[0012] In some embodiments of the present invention, a method for calculating the Markov transition probability of any grid q in a feasible state space includes the following steps: Determine the set that the grid q is currently in; If the grid , then the calculation formula of the Markov transition probability of grid q is: ; like Then, the calculation formula of the Markov transition probability from the grid q to the adjacent grid is: ; wherein, denotes the adjacent grid, denotes the k-step transition probability from the current grid q to the grid .

[0013] In some embodiments of the present application, is defined as: ; wherein, , , are all intermediate variables, , , the calculation formulae of which are respectively: ; ; ; wherein, is a time sampling interval, is a predetermined positive number, the value range of which is: ; wherein, is a drift term, is a diffusion term, n denotes the motion dimension, denotes a random disturbance error.

[0014] In some embodiments of the present application, based on the relative motion model of the unmanned ship and the expected trajectory point, is expressed as: ; is expressed as: ; wherein, denotes a position-related coefficient, denotes an attitude-related function, , , denotes the pose deviation of the unmanned ship and the expected trajectory point at the moment.

[0015] In some embodiments of the present application, the specific method for recursively calculating the unmanned ship trajectory tracking success rate in step S4 is: by taking the predetermined time interval Internal target set Back propagation Markov transition probability calculates the feasible state set at the current time t Each grid q in the future Time to reach the target set probability.

[0016] In some embodiments of the present invention, the specific method of recursively calculating the unmanned boat trajectory tracking success rate using the back propagation method in step S4 includes the following steps: Define the number of backpropagations ; Set the initial success probability of each grid to: ; for , the calculation formula for the success probability of each grid is: ; in, is the k-th step success probability of grid point q, and Respectively represent the kth step from the grid to itself and adjacent meshes The Markov transition probability of and Represents the grid and adjacent grids The success probability of the k+1th step; The success probability of any grid point is recursively calculated based on the initial value of the success probability of each grid and the calculation formula of the success probability of each grid.

[0017] The beneficial effects of the present invention are: The present invention establishes a random model of the relative motion between a modular unmanned boat and a desired trajectory, and introduces a spatial correlation function into the model to describe the influence of wind, wave and current disturbances on the relative posture of the two, laying the foundation for improving the accuracy of subsequent predictions; at the same time, the Markov random approximation theory is used to perform online prediction of the trajectory tracking success rate. Compared with the method of judging based on posture deviation in the existing technology, the accuracy of the prediction of the unmanned boat's trajectory tracking success rate is significantly improved, and it has higher credibility.

[0018] Other features and advantages of the present invention will be described in the following description, and in part will become apparent from the description, or will be understood by practicing the present invention. The purpose and other advantages of the present invention can be realized and obtained by the structures pointed out in the description, claims and drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0020] Figure 1 The schematic diagram of the structure of an online prediction method for trajectory tracking success rate of modular unmanned boats is shown in FIG. Figure 2 Schematic diagram of each set of average rasterization in the xy plane provided by an embodiment of the present invention; Figure 3 A schematic diagram of a Markov chain provided by an embodiment of the present invention; Figure 4 A schematic diagram of wind-wave and current disturbance provided by an embodiment of the present invention; Among them, (a) is the schematic diagram of wind-wave and current disturbance at t=0s, and (b) is the schematic diagram of wind-wave and current disturbance at t=6s; Figure 5 A schematic diagram of the trajectory tracking success rate prediction results in an undisturbed scenario provided by an embodiment of the present invention; Among them, (a) is the tracking success probability map in the gridded state space in the undisturbed scene, and (b) is the contour map of the tracking success probability in the undisturbed scene; Figure 6 A schematic diagram of the trajectory tracking success rate prediction results in a disturbed scenario provided by an embodiment of the present invention; Among them, (a) is the tracking success probability map in the gridded state space in the disturbed scene, and (b) is the contour map of the tracking success probability in the disturbed scene. DETAILED DESCRIPTION

[0021] In order to make the purpose, technical solutions and advantages of this application more clearly understood, the present application is described and illustrated below in conjunction with the accompanying drawings and examples. It should be understood that the specific embodiments described herein are merely used to explain this application and are not intended to limit this application. Based on the embodiments provided in this application, all other embodiments obtained by those of ordinary skill in the art without making any creative efforts are within the scope of protection of this application.

[0022] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0023] In order to better explain the solution of the present invention, the background technology of the present invention is first described.

[0024] Floating platforms are used for offshore production, operations, observation, and other activities, and play a vital role in marine engineering. Compared to traditional fixed floating platforms, floating platforms based on multiple modules in parallel 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 boats are autonomously moving intelligent entities. By using unmanned boats as modules, a reconfigurable floating platform based on the autonomous assembly of multiple unmanned boat modules (hereinafter referred to as modular unmanned boats) can be constructed, greatly expanding the application scenarios of such platforms.

[0025] Trajectory tracking is a key step in the deformation, reconstruction, and overall motion of modular unmanned vehicles (UAVs). It requires the platform to accurately track trajectory targets in real time. Considering that modular UAVs are typical overdriven systems, meaning the number of actuators exceeds the number of degrees of freedom, their trajectory tracking tasks include tracking both position and attitude (i.e., heading angle) in the horizontal plane. However, due to factors such as wind, waves, and currents at sea, modular UAVs may deviate from their original trajectory or even become dangerous. Therefore, online prediction of the 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 has failed to track the intended trajectory. Trajectory replanning, starting from the current position, is required to ensure that the mission can be successfully completed despite external influences.

[0026] To assess whether an unmanned aerial vehicle (UAV) is successfully tracking a desired trajectory, traditional approaches often directly rely on range and azimuth deviations. However, this approach is overly simplistic, fails to account for external disturbances, and lacks theoretical support. The success rate prediction method considers the randomness and uncertainty of the motion process and formulates the success rate prediction problem as a probabilistic reachability problem for a target set within the trajectory. This problem can be solved by deriving an analytical solution for the reachability of the target set. Currently, research on trajectory tracking success rate prediction is lacking, and the problem is somewhat similar to the problem of predicting the probability of aerial refueling docking. Trajectory tracking is a complex, nonlinear process involving a large number of process variables. Consequently, numerous numerical methods have been proposed to address the probabilistic reachability problem, primarily encompassing three categories: model methods, limit approximation methods, and data-driven methods. Model methods involve simplifying and abstracting the original complex process. For example, they use a grid to discretize the state space and generate a Markov chain as an approximation of the complex process, then calculate the probability of the system's current state reaching the target set. Limit approximation methods typically approximate the reachable set using compact forms such as polyhedra, hyperrectangles, ellipsoids, or polygons, obtaining an effective limit approximation of the reachable set. Data-driven methods train deep networks to extract useful feature information and can directly predict docking success rates. Existing methods each have their own advantages and disadvantages. Model methods and limit approximation methods, while computationally efficient, can be overly conservative. Data-driven methods, while capable of fitting complex processes, require a large amount of real-world experimental data. Furthermore, limited research has focused directly on modular unmanned vehicles. Given the dynamic nature of the target set, the large state space, and the long forward prediction time involved in trajectory tracking, directly applying existing success rate prediction methods can lead to a series of challenges, such as a lack of integration with the actual control scheme and motion model, and overly dense prediction results.

[0027] In the absence of conflict, the embodiments of the present invention and the features thereof may be combined with each other.

[0028] The technical solution of the present invention is described in detail below in conjunction with specific embodiments and the accompanying drawings.

[0029] As attached Figure 1 -Attached Figure 6 As shown, in an illustrative embodiment of an online prediction method for trajectory tracking success rate of a modular unmanned boat of the present invention, the prediction method includes the following steps.

[0030] S1: A stochastic differential equation is used to describe the motion of the modular unmanned boat and its desired trajectory, and then a relative motion model of the unmanned boat and the desired trajectory point is established based on the motion formula of the modular unmanned boat and its desired trajectory.

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

[0032] S11: Use stochastic differential equations to describe the motion of modular unmanned vehicles and their desired trajectories.

[0033] Specifically, the motion formula of the modular unmanned boat is expressed as: .

[0034] The motion formula of the desired trajectory is expressed as: .

[0035] in, and is the state vector representing the current state, the state vector , 、 、 Represent the lateral distance, longitudinal distance and heading angle of the unmanned boat respectively, and the state vector , 、 、 Respectively represent the lateral distance, longitudinal distance and heading angle of the desired track point; Indicates the speed of the unmanned boat. , 、 、 Respectively represent the lateral speed, longitudinal speed and bow speed of the unmanned boat; represents the movement speed of the desired trajectory point, , 、 、 They represent the lateral velocity, longitudinal velocity and heading velocity of the desired trajectory point respectively; is a function that represents the disturbance of wind, waves and currents on the unmanned boat, ; is a function that represents the disturbance of wind, wave and current on the desired trajectory point, ; represents a third-order matrix, represents a diagonal matrix whose diagonal elements are time-dependent; represents the variance of the random disturbance, , 、 、 denote the random disturbance variances in the transverse, longitudinal and heading directions respectively; and are functions representing random disturbances.

[0036] It should be noted that the random perturbation is not a standard Brownian motion, but is represented as and In some embodiments of the present invention, the function representing random perturbation is defined as , The formula for calculating the covariance of is: ; in, is the third-order identity matrix, is the posture correlation function. When the posture of the unmanned boat is consistent with the expected trajectory point, ,but ; When the position of the unmanned boat is inconsistent with the expected trajectory point, ,but .

[0037] S12: Subtract the motion formula of the modular unmanned boat from the motion formula of the desired trajectory to obtain the relative motion model of the unmanned boat and the desired trajectory point: .

[0038] in, represents the position of the unmanned boat relative to the desired trajectory point, , represents the speed of the unmanned boat relative to the desired trajectory point, , It represents the difference between the random disturbance on the unmanned boat and the random disturbance at the expected trajectory point.

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

[0040] S13: Based on the relative motion model between the unmanned boat and the desired trajectory point, Defined as: .

[0041] but The covariance of is expressed as: .

[0042] based on The covariance of The expression is: .

[0043] in, represents the spatial correlation function, and The expression is: .

[0044] in, represents the position correlation coefficient, It represents the attitude correlation function. The values ​​of the position correlation coefficient and attitude correlation function reflect the degree of correlation between the relative posture between the unmanned boat and the trajectory point and the wind, wave and current disturbance.

[0045] S14: Based on The relative motion model between the unmanned boat and the desired trajectory point is re-described as: .

[0046] in is the standard Brownian motion, is the drift term, is the diffusion term.

[0047] S2: Divide the feasible state space of the unmanned boat into multiple non-intersecting sets. The multiple sets include but are not limited to the target set , feasible state set and the feasible state boundary set .

[0048] Specifically, the expected state of the unmanned boat at time t is defined as The expected trajectory point at the moment, the area around the trajectory point is defined as the target set According to the task constraints, the other state spaces can be divided into the set of feasible states , feasible state boundary set The size of the area around the track point depends on the maximum allowable value of the tracking error and needs to be set in advance.

[0049] The spatial areas in each set are scale-adaptively rasterized.

[0050] Specifically, for regions far from the target set or feasible state boundary set, a sparse Markov chain can be constructed for fast calculations, resulting in a sparse grid, i.e., a larger grid size per degree of freedom. Regions closer to the target set or feasible state boundary set require a dense Markov chain for accurate calculations, thus a denser grid, i.e., a smaller grid size.

[0051] In some embodiments of the present invention, in step S2, during the scale-adaptive rasterization process of the spatial regions within each set, the size of each grid in the x-axis direction is defined as: ; in, Indicates the distance between the current grid and the target set on the x-axis, Indicates the distance between the current grid and the feasible state boundary set on the x-axis, Indicates the number of grids on the x-axis; The size of each grid in the y-axis direction is defined as: ; in, Indicates the distance between the current grid and the target set on the y-axis, Indicates the distance between the current grid and the feasible state boundary set on the y-axis, Indicates the number of grids on the y-axis; Each grid The dimensions in the axial direction are defined as: ; in, express The distance between the current grid and the target set on the axis, express The distance between the current grid and the feasible state boundary set on the axis, express The number of grids on the axis, The axis direction is perpendicular to the x-axis and y-axis. Subsequently, the grid size in each axis direction can be uniformly described as ,in , For standard scale, is the proportional coefficient.

[0052] The above grid size strategy can take into account both computational efficiency and accuracy.

[0053] In a specific embodiment of the present invention, schematic diagrams of the average gridded sets in the xy plane are as follows: Figure 2 As shown in the figure. Since the trajectory tracking task requires the relative position of the unmanned boat and the desired trajectory to be as close to 0 as possible, in the target set The longitudinal / lateral distance and heading angle of the unmanned boat relative to the desired trajectory point can be used as relevant states. , the relative state of the modular unmanned boat at time t is , then the prediction The trajectory tracking success rate at time can be expressed as .

[0054] S3: Based on the relative motion model, the Markov transition probability of any grid in the feasible state space is calculated to obtain the Markov chain model.

[0055] Based on the gridded space region established in step S2, a Markov chain is established to describe the state transition model of the unmanned boat. Specifically, Figure 3 As shown, for a grid q , you can The adjacent grids in the direction and the transfer of itself, its adjacent grid set include , , , , , ,in , , With the diagonal matrix Elements in Related, expressed as and .

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

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

[0058] If the grid , then the calculation formula of the Markov transition probability of grid q is: .

[0059] This calculation formula shows that the grid q belonging to the feasible state boundary set will not be transferred to other grids because the set is an absorbing region.

[0060] like , then the calculation formula of the Markov transition probability from grid q to the adjacent grid is: .

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

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

[0063] in, 、 、 are intermediate variables, and 、 、 The calculation formulas are: ; ; .

[0064] in, is the time sampling interval, is a predetermined positive number, The value range of is: .

[0065] in, is the drift term, is the diffusion term, n represents the motion dimension, represents the random disturbance error.

[0066] In some embodiments of the present invention, based on the relative motion model between the unmanned boat and the desired trajectory point, Expressed as: .

[0067] Expressed as: .

[0068] in, represents the position correlation coefficient, represents the posture-related function, 、 、 express The position deviation between the unmanned boat and the expected trajectory point at that moment.

[0069] The smaller the time sampling interval, the higher the accuracy of the above transition probability calculation and relative motion model, but the computational complexity 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 vehicle and the desired trajectory point.

[0070] S4: Based on the Markov transition probability and Markov chain model, the back propagation method is used to recursively calculate the trajectory tracking success rate of the unmanned boat; when the trajectory tracking success rate of the unmanned boat at the current position is lower than the predetermined threshold, the desired trajectory of the unmanned boat is replanned.

[0071] In some embodiments of the present invention, the specific method of recursively calculating the unmanned boat trajectory tracking success rate using the back propagation method in step S4 is: According to the Markov chain and transition probability calculation formula established in step S3, we use the back propagation method to predict the success rate of unmanned boat trajectory tracking , that is, by Internal target set Back propagation Markov transition probability calculates the feasible state set at the current time t Each grid q in the future Time to reach the target set probability.

[0072] In some embodiments of the present invention, the specific method of recursively calculating the unmanned boat trajectory tracking success rate using the back propagation method in step S4 includes the following steps.

[0073] Define the number of backpropagations .

[0074] Set the initial success probability of each grid to: .

[0075] for , the calculation formula for the success probability of each grid is: .

[0076] in, is the k-th step success probability of grid point q, and Respectively represent the kth step from the grid to itself and adjacent meshes The Markov transition probability of and Represents the grid and adjacent grids The success probability of the k+1th step. is an absorption area, when When , the success rate of the grid point is 1. Correspondingly, the feasible state boundary set It is also an absorption area. When , the success rate of the grid point is 0.

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

[0078] The above calculation method determines the UAV's trajectory tracking success rate at the current grid point based on the UAV's current location. When this value falls below a safety threshold, the UAV must perform online trajectory replanning. This means abandoning the current trajectory and having the planner replan a new trajectory starting from the current point, and then tracking the new trajectory.

[0079] In a specific embodiment of the present invention, comparative experiments were conducted in both non-disturbance and disturbance scenarios, respectively, to verify the effectiveness of the prediction method provided by the present invention.

[0080] If the influence of wind, wave and current disturbance is not considered, it means .

[0081] If the influence of wind, wave and current disturbance is considered, the disturbance function The specific form is: , It can be viewed as a clockwise rotating storm whose center moves with time, e.g. Figure 4As 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.

[0082] Taking the expected trajectory point at a certain moment as an example, the two circles of grids around this point are defined as the target set. The success rate calculation results of the unmanned boat tracking the trajectory point at different positions (i.e., the full state space) under no disturbance and disturbance conditions are as follows: Figure 5 and 6 As shown in Figure 2, the experimental results are divided into two parts: Figure 5 (a) and Figure 6 (a) is the tracking success probability graph in the grid state space, Figure 5 (b) and Figure 6 (b) is a contour map of the tracking success probability. In the contour map, the square at the origin represents the target set, and its outer envelope represents the probability contour line. The tracking success rate of the state grids on the same contour line is equal. Figure 5 and Figure 6 It can be seen that since the wind, wave and current disturbances show a clockwise trend, the contour map of the tracking success probability under disturbance will bend counterclockwise compared to the result without disturbance.

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

[0084] The above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit the same. Although the present invention has been described in detail with reference to preferred embodiments, persons skilled in the art should understand that the specific implementation methods of the present invention may still be modified or some technical features may be replaced by equivalents without departing from the spirit of the technical solutions of the present invention, and all of these should fall within the scope of the technical solutions claimed for protection by the present invention.

Claims

1. An online prediction method for trajectory tracking success rate of a modular unmanned boat, characterized in that: The following steps are involved: S1: Use stochastic differential equations to describe the motion of the modular unmanned boat and its desired trajectory, and then establish a relative motion model between the unmanned boat and the desired trajectory points based on the motion formula of the modular unmanned boat and its desired trajectory; S2: Divide the feasible state space of the unmanned boat into multiple non-overlapping sets; the multiple sets include but are not limited to the target set , feasible state set and the feasible state boundary set ; Perform scale-adaptive rasterization on the spatial regions within each set; S3: Calculating 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 back propagation method is used to recursively calculate the success rate of the unmanned boat trajectory tracking.

2. The online prediction method for trajectory tracking success rate of a modular unmanned boat according to claim 1, characterized in that: Step S1 specifically includes the following steps: S11: Using stochastic differential equations to describe the motion of modular unmanned vehicles and their desired trajectories; The motion formula of the modular unmanned boat is expressed as: ； The motion formula of the desired trajectory is expressed as: ； in, and is the state vector representing the current state, the state vector , 、 、 Represent the lateral distance, longitudinal distance and heading angle of the unmanned boat respectively, and the state vector , 、 、 Respectively represent the lateral distance, longitudinal distance and heading angle of the desired track point; Indicates the speed of the unmanned boat. , 、 、 They represent the lateral speed, longitudinal speed and bow speed of the unmanned boat respectively; represents the movement speed of the desired trajectory point, , 、 、 represent the lateral velocity, longitudinal velocity and heading velocity of the desired trajectory point respectively; is a function that represents the disturbance of wind, waves and currents on the unmanned boat, ; is a function that represents the disturbance of wind, wave and current on the desired trajectory point, ; represents a diagonal matrix whose diagonal elements are time-dependent; represents the variance of the random disturbance, , 、 、 denote the random disturbance variances in the transverse, longitudinal and heading directions respectively; and Both are functions representing random disturbances; S12: Based on the motion formula of the modular unmanned boat and the motion formula of the desired trajectory, a relative motion model between the unmanned boat and the desired trajectory point is established: ； in, represents the position of the unmanned boat relative to the desired trajectory point, , represents the speed of the unmanned boat relative to the desired trajectory point, , It represents the difference between the random disturbance on the unmanned boat and the random disturbance at the expected trajectory point.

3. The online prediction method for trajectory tracking success rate of a modular unmanned boat according to claim 2, characterized in that: Step S1 further includes the following steps: S13: Based on the relative motion model between the unmanned boat and the desired trajectory point, Defined as: ； but The covariance of is expressed as: ； Based on the The covariance of The expression is: ； in, represents the spatial correlation function, and The expression is: ； in, represents the position correlation coefficient, represents the posture-related function; S14: Based on the The relative motion model between the unmanned boat and the desired trajectory point is re-described as: ； in is the standard Brownian motion, is the drift term, is the diffusion term.

4. The online prediction method for trajectory tracking success rate of a modular unmanned boat according to claim 2, characterized in that: The function representing the random perturbation is defined as , The formula for calculating the covariance of is: ； in, is the third-order identity matrix, is the posture correlation function. When the posture of the unmanned boat is consistent with the expected trajectory point, ,but ; When the position of the unmanned boat is inconsistent with the expected trajectory point, ,but .

5. The online prediction method for trajectory tracking success rate of a modular unmanned boat according to any one of claims 1 to 4, characterized in that: In step S2, during the scale-adaptive rasterization process of the spatial regions within each set, the size of each grid in the x-axis direction is defined as: ； in, Indicates the distance between the current grid and the target set on the x-axis, Indicates the distance between the current grid and the feasible state boundary set on the x-axis, Indicates the number of grids on the x-axis; The size of each grid in the y-axis direction is defined as: ； in, Indicates the distance between the current grid and the target set on the y-axis, Indicates the distance between the current grid and the feasible state boundary set on the y-axis, Indicates the number of grids on the y-axis; Each grid The dimensions in the axial direction are defined as: ； in, express The distance between the current grid and the target set on the axis, express The distance between the current grid and the feasible state boundary set on the axis, express The number of grids on the axis, An axis direction is perpendicular to the x-axis direction and the y-axis direction; The grid size in each axis direction is defined as ; in , For standard scale, is the proportional coefficient.

6. The online prediction method for trajectory tracking success rate of a modular unmanned boat according to claim 5, characterized in that: The method for calculating the Markov transition probability of any grid q in the feasible state space comprises the following steps: Determine the set that the grid q is currently in; If the grid , then the calculation formula of the Markov transition probability of grid q is: ； like , then the calculation formula of the Markov transition probability from grid q to the adjacent grid is: ； in, represents adjacent grids, Indicates the transition from the current grid q to the grid The k-th step transition probability.

7. The online prediction method for trajectory tracking success rate of a modular unmanned boat according to claim 6, characterized in that: described Defined as: ； in, 、 、 are intermediate variables. 、 、 The calculation formulas are: ； ； ； in, is the time sampling interval, is a predetermined positive number, The value range of is: ； in, is the drift term, is the diffusion term, n represents the motion dimension, represents the random disturbance error.

8. The online prediction method for trajectory tracking success rate of a modular unmanned boat according to claim 7, characterized in that: Based on the relative motion model between the unmanned boat and the expected trajectory point, the Expressed as: ； described Expressed as: ； in, represents the position correlation coefficient, represents the posture-related function, 、 、 express The position deviation between the unmanned boat and the expected trajectory point at that moment.

9. The online prediction method for trajectory tracking success rate of a modular unmanned boat according to claim 1 or 8, characterized in that: The specific method of recursively calculating the unmanned boat trajectory tracking success rate using the back propagation method in step S4 is: By Internal target set Back propagation Markov transition probability calculates the feasible state set at the current time t Each grid q in the future Time to reach the target set probability.

10. The online prediction method for trajectory tracking success rate of a modular unmanned boat according to claim 9, characterized in that: The specific method of recursively calculating the unmanned boat trajectory tracking success rate using the back propagation method in step S4 includes the following steps: Define the number of backpropagations ; Set the initial success probability of each grid to: ； for , the calculation formula for the success probability of each grid is: ； in, is the k-th step success probability of grid point q, and Respectively represent the kth step from the grid to itself and adjacent meshes The Markov transition probability of and Represents the grid and adjacent grids The success probability of the k+1th step; The success probability of any grid point is recursively calculated based on the initial value of the success probability of each grid and the calculation formula of the success probability of each grid.

Images