Optimal trajectory planning method and system for unmanned surface vehicle in complex environment

Abstract

The application discloses a method and system for optimal trajectory planning of an unmanned surface vehicle (USV) in a complex environment, wherein the method steps include: state initialization of a target to obtain initial state information; construction of a system model based on the initial state information; construction of a threat-based knowledge graph and reasoning to obtain threat degree information; construction of evaluation indexes in combination with multi-target posterior Cramer-Rao lower bound and the threat degree information; and optimal trajectory planning based on the evaluation indexes. The application combines target and ocean current movement situation to construct a knowledge graph based on marine threats. The threat semantic information in the graph is reasoned through a reinforcement learning method, which effectively solves the shortcomings of traditional threat evaluation methods lacking self-learning ability for threat characteristics and can deeply mine the correlation between evaluation indexes. The application can effectively avoid target threats and ocean current disturbances while optimizing target tracking effect.

Description

Technical Field

[0001] This invention relates to the field of radar signal processing technology, specifically to a method and system for optimal trajectory planning of unmanned surface vehicles in complex environments. Background Technology

[0002] With the continuous advancement of marine resource utilization technologies, countries are increasingly emphasizing maritime security, making the ocean a strategic focus of modern warfare. USVs (Unmanned Surface Vehicles), with their advantages of stealth, scalability, and flexibility, are widely used in maritime reconnaissance and early warning, target monitoring, and anti-submarine warfare missions. However, due to the complex and ever-changing marine environment, USV navigation and control are easily affected by external interferences such as time-varying sea winds, ocean currents, and waves. Therefore, effectively ensuring their navigation safety has become a key factor for mission success. Real-time planning of USV navigation paths to adapt to environmental changes is of great significance for maritime monitoring missions and maintaining maritime security. With the rapid development of artificial intelligence technology, unmanned surface navigation systems not only possess information perception capabilities but also logical reasoning, inferential decision-making, and adaptive capabilities. In particular, how to achieve intelligent decision-making in complex environments has become a key research focus.

[0003] In the path planning problem of unseen vehicles (USV), reinforcement learning (RL), as a trial-and-error-based learning method, learns the optimal policy through the interaction between the agent and the environment, and has become an effective framework for solving sequential decision-making tasks such as path planning. Traditional RL algorithms perform well in low-dimensional action spaces, but often encounter the dimensionality explosion problem when facing high-dimensional action spaces. With the development of deep learning technology, deep reinforcement learning (DRL) combines the advantages of deep learning in high-dimensional data processing and feature extraction with the intelligent decision-making capabilities of reinforcement learning, providing a new solution to the path planning problem in complex environments.

[0004] Traditional threat assessment methods typically rely on expert experience to determine threat indicators and build models. However, these methods involve complex theoretical knowledge, the process of building and adjusting models is cumbersome, and they also have high requirements for data quality and completeness. Poor data quality or significant data gaps can affect the accuracy and reliability of the assessment results. Therefore, how to establish more dynamic and real-time intelligent threat assessment methods has become a major direction in current threat assessment research. In recent years, knowledge graphs have become a research focus due to their ability to deeply mine and analyze potential threat information in complex knowledge and provide a new perspective on entity relationships to describe multi-source heterogeneous data. Summary of the Invention

[0005] To address the above technical problems, this invention provides a method for optimal trajectory planning of unmanned surface vehicles in complex environments, comprising the following steps:

[0006] The target unmanned surface vehicle is initialized to obtain initial state information;

[0007] Based on the initial state information, a system model is constructed;

[0008] Based on the system model, a threat-based knowledge graph is constructed, and a policy gradient reinforcement learning algorithm is used to infer the threat level to obtain the inference result.

[0009] Based on the reasoning results, an evaluation index is constructed;

[0010] Based on the aforementioned evaluation metrics, the optimal trajectory is planned.

[0011] Preferably, the method for performing the state initialization includes: setting the multi-target state set of t targets at time k as... The sampling period is T = 1s, and the process noise matrix is ​​v. k Assume that m measurements are received at time k, and the multi-target measurement set Z k for Measurement noise matrix w k The number of sampling points is N=100.

[0012] Preferably, the system model includes: the target's state transition equation, the target's measurement equation, and an ocean current model; wherein, the target's state transition equation includes:

[0013]

[0014] Where f(·) represents the state evolution mapping; v k Indicates process noise;

[0015] The measurement equations for the target include:

[0016]

[0017] Where, x s,k =[x s,k ,y s,k ] T The position of the USV at time k is indicated; h represents the measurement mapping; x s,k The x-coordinate of USV at time k; y s,k The ordinate of USV at time k is represented;

[0018] The ocean current model includes:

[0019]

[0020] Among them, u c This indicates the velocity of the ocean current in the y-direction; v c S represents the velocity of the ocean current in the x-direction.c =[x c ,y c ] T S represents the displacement vector of ocean currents. o (ΔT A )=[x o ,y o ] T ζ represents the vortex center vector; ζ represents the vortex intensity; l represents the vortex radius.

[0021] Preferably, the method for reasoning about threat level using policy gradient reinforcement learning algorithm includes: constructing a threat assessment dataset, then training a supervised policy network by exploring a given positive sample subset, and then, based on the supervised policy, enabling the agent to learn actions that maximize the reward function to further explore the path of threat entities from the threat assessment dataset, thereby realizing the reasoning process for threat level.

[0022] Preferably, the method for constructing the evaluation index includes: calculating the Cramerlow lower bound of all targets based on state priors and covariance matrices to evaluate the performance of multi-target tracking; then inferring the threat level in real time and calculating the distance between the USV and the target to determine the threat level of each target and ocean current.

[0023] Preferably, the method for completing the optimal trajectory planning includes: using the SAC agent to continuously interact with the multi-target tracking and filtering environment to obtain the control data of the unmanned surface vehicle to train the agent; and finally obtaining the action sequence that maximizes the reward function through the trained SAC agent to determine the optimal trajectory of the unmanned surface vehicle.

[0024] This invention also provides an optimal trajectory planning system for unmanned surface vehicles in complex environments. The system is used to implement the above method and includes: an initialization module, a construction module, a reasoning module, an evaluation module, and a planning module.

[0025] The initialization module is used to initialize the state of the target unmanned surface vehicle and obtain initial state information.

[0026] The construction module is used to construct a system model based on the initial state information;

[0027] The reasoning module is used to construct a threat-based knowledge graph based on the system model, and to use a policy gradient reinforcement learning algorithm to reason about the threat level to obtain the reasoning result;

[0028] The evaluation module is used to construct evaluation indicators based on the reasoning results;

[0029] The planning module is used to complete the optimal trajectory planning based on the evaluation index.

[0030] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0031] This paper proposes an intelligent decision-making method based on deep reinforcement learning for USV path planning in complex environments. This method constructs a knowledge graph based on ocean threats by combining target information and ocean current dynamics. Reinforcement learning is then used to reason about the semantic information of threats in the graph, effectively addressing the shortcomings of traditional threat assessment methods that lack self-learning capabilities for threat features and enabling in-depth exploration of the correlations between assessment indicators. Furthermore, threat information and the posterior Cramérod lower bound of multiple targets are used as the joint reward function for USV path planning, effectively avoiding target threats and ocean current disturbances while optimizing target tracking performance. Attached Figure Description

[0032] To more clearly illustrate the technical solution of the present invention, the drawings used in the embodiments are briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0033] Figure 1 This is a schematic diagram of the method flow according to an embodiment of the present invention;

[0034] Figure 2 This is a flowchart illustrating the path planning process according to an embodiment of the present invention.

[0035] Figure 3 This is a schematic diagram of path planning for USV monitoring targets in a complex marine environment, according to an embodiment of the present invention.

[0036] Figure 4 This is a schematic diagram illustrating the root mean square error (RMSE) of the USV tracking target for three targets in an embodiment of the present invention; wherein, Figure 4 (a), (b), and (c) show the trends of the errors of the three targets over the first 50 time steps.

[0037] Figure 5 This is a schematic diagram of the target threat distribution density in an embodiment of the present invention;

[0038] Figure 6 This is a schematic diagram showing the angle between the USV's heading and the ocean current direction in an embodiment of the present invention. Detailed Implementation

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

[0040] It should be noted that, unless otherwise defined, the technical or scientific terms used in the embodiments of this disclosure should have the ordinary meaning understood by one of ordinary skill in the art to which this disclosure pertains. The terms "first," "second," and similar terms used in the embodiments of this disclosure do not indicate any order, quantity, or importance, but are merely used to distinguish different components. Terms such as "comprising" or "including" mean that the element or object preceding the word encompasses the elements or objects listed following the word and their equivalents, without excluding other elements or objects. Terms such as "connected" or "linked" are not limited to physical or mechanical connections, but can include electrical connections, whether direct or indirect. Terms such as "upper," "lower," "left," and "right" are used only to indicate relative positional relationships; when the absolute position of the described object changes, the relative positional relationship may also change accordingly.

[0041] First, some technical terms used in this invention will be explained:

[0042] USV: Unmanned surface vehicle.

[0043] SAC, or "SoftActor-Critic," is a deep reinforcement learning algorithm. Combining the ideas of maximum entropy reinforcement learning and Q-learning, SAC aims to learn a policy by maximizing a function that incorporates expected reward and entropy. This approach encourages the agent to explore the environment while optimizing its behavior to obtain higher rewards. SAC performs particularly well in problems with continuous action spaces because it can handle high-dimensional, continuous control problems and exhibits good sample efficiency and stability.

[0044] PCRLB stands for Posterior Cramér-Rao Lower Bound. It is a tool for measuring the estimation accuracy of nonlinear filtering problems. It can provide a lower bound for the target state estimation error at the next time step, and is therefore often used as an evaluation criterion for moving target tracking error.

[0045] PG stands for "Policy Gradient," a type of reinforcement learning algorithm. The policy gradient method learns a policy directly by optimizing the parameters of a policy function, which defines the probability of choosing each possible action given a state. This method increases the probability of actions that lead to higher cumulative rewards by calculating gradients and updating policy parameters.

[0046] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0047] Example 1

[0048] As the background technology indicates, traditional threat assessment methods typically rely on expert experience to determine threat indicators and build models. However, these methods involve complex theoretical knowledge, the process of building and adjusting models is cumbersome, and they also have high requirements for data quality and completeness. Poor data quality or significant data gaps can affect the accuracy and reliability of the assessment results. Therefore, how to establish more dynamic and real-time intelligent threat assessment methods has become a major direction in current threat assessment research. In recent years, knowledge graphs have become a research focus due to their ability to deeply mine and analyze potential threat information in complex knowledge and provide a new perspective on entity relationships to describe multi-source heterogeneous data.

[0049] This invention provides a method for optimal trajectory planning of unmanned surface vehicles in complex environments, such as... Figure 1 As shown, the steps include:

[0050] S1. Initialize the state of the target unmanned surface vehicle to obtain initial state information.

[0051] Let the multi-objective state set of t objectives at time k be... The sampling period is T = 1s, and the process noise matrix is ​​v. k Assume that m measurements are received at time k, and the multi-target measurement set Z k for Measurement noise matrix w k The sampling points are N=100, and the initial state information is obtained in this way.

[0052] S2. Construct a system model based on the initial state information.

[0053] Based on the initial state information of the target, construct the target's state transition equation and measurement equation; and construct a mathematical model of ocean current changes based on numerical estimation of the Lamb and Navier-Stokes equations. The steps include:

[0054] (1) Modeling a multi-target tracking system

[0055] Let the state transition equation of the target at time k be:

[0056]

[0057] Where f(·) represents the state evolution mapping; v k This indicates process noise.

[0058] Assume the observation noise is w k The corresponding measurement equation is:

[0059]

[0060] Where, x s,k =[x s,k ,y s,k ] T The position of the USV (Unmanned Surface Vehicle) at time k is represented by h; the measurement map is represented by x. s,k The x-coordinate of USV at time k; y s,k The vertical coordinate of USV at time k is represented.

[0061] (2) Constructing an ocean current model

[0062]

[0063] Among them, u c This indicates the velocity of the ocean current in the y-direction; v c S represents the velocity of the ocean current in the x-direction. c =[x c ,y c ] T S represents the displacement vector of ocean currents. o (ΔT A )=[x o ,y o ] T ζ represents the vortex center vector; ζ represents the vortex intensity; l represents the vortex radius.

[0064] S3. Based on the system model, construct a threat-based knowledge graph and use the policy gradient reinforcement learning algorithm to reason about the threat level to obtain the reasoning results.

[0065] A threat assessment dataset is constructed, and then a supervised policy network is trained by exploring more effectively from a given positive sample subset. The knowledge in the positive sample subset encourages the PG's agent to explore entities that are beneficial to its policy learning. Then, based on the supervised policy, the agent learns actions that maximize the reward function to further explore paths to threatening entities from the threat assessment dataset, thus realizing the inference process regarding threat levels.

[0066] Specifically, a filtering environment based on multi-target tracking and deep reinforcement learning interaction is constructed. A threat-based knowledge graph is built by combining target and ocean current movement patterns, and reinforcement learning is used to infer threat levels. The steps include:

[0067] S301. Prior state of target t at time k and state prior covariance matrix Described as:

[0068]

[0069]

[0070] Among them, F k|k-1 This is the state transition matrix; Let be the process noise covariance matrix.

[0071] After predicting the state in one step, select action a according to the policy. k USV from position x s，k-1 Move to x s，k The corresponding measurement information will be affected. Based on the measurement information after the USV moves, the posterior estimate of the target t's state can be obtained as follows:

[0072]

[0073] Among them, K k Represents the system gain matrix; Indicates the execution of action a k The i-th measurement The probability of association with target t, where i = 0 indicates that no measurement was performed. From target t; It indicates new information.

[0074] The posterior state covariance matrix of target t is:

[0075]

[0076] in, This represents the covariance matrix used to predict the uncertainty of the target state at time k based on the state estimate at time k-1. This indicates that USV is at position x s，k The new information covariance matrix; This represents the joint innovation of the target state estimated using all pseudo-measures under the associated probability.

[0077] After the S302.USV moves to the appropriate location, it obtains the data source for threat assessment using a knowledge graph and constructs a threat knowledge graph. Assume there are t targets and n ocean currents within the USV's monitoring area. Therefore, the set of head entity vectors at each time point can be represented as:

[0078]

[0079] in Let t represent the t-th target. This represents the nth ocean current.

[0080] The sets of relation vectors and tail entity vectors are as follows:

[0081]

[0082]

[0083] Where u represents the number of threat-related attributes, including target location, target velocity, target angle relative to the USV, and ocean current angle relative to the USV; This represents the u-th relation vector at time k. Let represent the u-th tail entity vector of target t at time k.

[0084] Based on the measurement information obtained from the radar, the position and angle of target t at time k are as follows:

[0085]

[0086]

[0087] Based on the state estimation information, the velocity tail entity of target t is:

[0088]

[0089] in, and These represent the target's lateral velocity and longitudinal velocity, respectively.

[0090] The angle between the ocean current and the USV can be calculated using the following formula:

[0091]

[0092] In summary, the set of positive samples is E. + ={H k L k P k To improve the accuracy and robustness of the algorithm, some triples are replaced with head or tail entities to generate negative samples. The knowledge base containing positive and negative samples forms a dataset E related to the target threat, denoted as:

[0093] E = E + ∪E - (15)

[0094] Among them, E+ and E - These represent the positive sample set and the negative sample set, respectively.

[0095] S303. For each relation, a subset of all positive samples is used to learn a supervised policy. In each positive sample (e source e target In this paper, a bidirectional path constraint search (BFS) is used to determine the correct path between entities. Each path P contains a sequence of relations l1→l2→...→l n The parameter θ is updated using the Monte Carlo policy gradient method.

[0096] In supervised learning, a reward is given for each successful learning step. The approximate gradient update formula for the policy network, substituted into the correct path found, is as follows:

[0097]

[0098] Among them, s t ' represents the state of the input supervision policy at time t; J represents the path l t The sum of expected rewards; a represents the gradient with respect to the parameter θ; t ′ represents the action output by the supervision policy at time t; A represents the action space; l t ∈p.

[0099] Furthermore, through the pre-generated training set E tr The supervised policy network is then retrained using PG reinforcement learning to determine the inference path controlled by the reward function. In a knowledge graph environment, the agent's primary task is to determine whether the target entity e can be reached through a series of paths. target Therefore, the following accuracy reward function r was designed. GLOBAL :

[0100]

[0101] When an agent reaches the target entity after performing a series of actions, it receives a reward of +1; otherwise, it receives a reward of -1.

[0102] For each relation, the reasoning of an entity pair constitutes an act. Starting from the source node, the agent selects a relation to extend its reasoning path based on a random policy and the probability distribution of all relations. This random policy avoids the agent repeatedly reasoning in incorrect relations; upon failure, the agent remains on the original entity node, maintaining the same state. An act ends if the agent reaches the target entity within the maximum number of steps or fails to do so. After each act, the policy network is updated using the following gradients:

[0103]

[0104] Where J represents path l t The sum of expected rewards; a represents the gradient with respect to the parameter θ; t ' represents the action performed by the PG agent at time t (the relation node selected by the agent in the knowledge graph); s t ′ represents the state of the PG agent in the knowledge graph at time t.

[0105] To improve training efficiency, this invention uses an experience playback mechanism to store historical data in an experience pool for subsequent learning, and updates the experience pool every 100 times.

[0106] S4. Based on the reasoning results, construct evaluation indicators.

[0107] To meet the mission requirements of the USV, the reward function design needs to consider optimizing tracking performance and avoiding threats, prioritizing the tracking of targets with the highest threat. Specifically, based on state priors and covariance matrices, Cramer-Rao lower bounds for all targets are calculated to evaluate the performance of multi-target tracking. Threat levels are inferred in real time, and the distance between the USV and targets is calculated to determine the threat level of each target and ocean current, thereby avoiding target threats and ocean current disturbances, and prioritizing the tracking of high-threat targets. The specific steps are as follows:

[0108] Constructing evaluation metrics for path planning using multi-objective posterior Cramerlow lower bounds and threat information:

[0109] Assuming based on measurement The unbiased estimate of the state of target t is The following inequalities must be satisfied:

[0110]

[0111] in, This represents the true state of target t at time k; for Fisher Information Matrix (FIM).

[0112] The target at time k corresponds to It can be calculated as follows:

[0113]

[0114] in, The matrix representing the prior state information of the target; Indicates based on measurement Fisher's information matrix.

[0115] Let the prior state information matrix of the target be the recursive formula for the information matrix, which can be expressed as:

[0116]

[0117]

[0118] in, This represents the target state at time k-1. Fisher's information matrix; This represents the target state at time k-1. arrive Fisher's information matrix; express arrive Fisher's information matrix; Represents the target state at time k. Fisher's information matrix.

[0119] The data information matrix representing the target is shown below:

[0120]

[0121] in, This indicates that the target state is given at time k. Under the conditions, observation The probability density function.

[0122] From the system's state equation and measurement equation, we can obtain:

[0123]

[0124]

[0125] in, This is the state transition matrix; This represents the inverse of the posterior information matrix at time k-1; The process noise covariance matrix; Represents the observation matrix; This represents the observation noise covariance matrix.

[0126] From equations (24) and (25), we can see that the lower bound of Cramer-Rayet is... The expression is as follows:

[0127]

[0128] The diagonal elements of PCRLB provide a lower bound for the variance of each component in the estimated target state vector. In this embodiment, the tracking performance of the target motion state is evaluated by comparing the root mean square error of the target state estimation with the PCRLB. The closer the root mean square error is to the lower bound, the better the overall tracking performance. The expression is as follows:

[0129] r track =RMSE-PCRLB (27)

[0130] Where RMSE is the root mean square error of the target state estimation, and PCRLB is the posterior Cramero lower bound. Since threat levels in knowledge graph reasoning are categorized into high, medium, and low, the threat reward function defined in this embodiment is as follows to quantify threat levels:

[0131]

[0132] Where, η penalty =1 represents the penalty coefficient. When the threat level is `threat_low`, the threat reward is 0; otherwise, the threat reward is less than 0. Furthermore, USVs performing maritime surveillance missions need to collect information on targets with the highest threat level. Therefore, the path planning should prioritize tracking targets with the highest threat level. For this purpose, the distance between the USV and the target is used as the evaluation metric. The distance reward function is as follows:

[0133]

[0134] In summary, the following global reward function is constructed:

[0135] r k =R(a k )=ω1·r track +ω2·r threat +ω3·r dist (30)

[0136] S5. Based on the evaluation indicators, complete the optimal trajectory planning.

[0137] The SAC agent continuously interacts with the multi-target tracking filtering environment to obtain USV control data for training. The trained SAC agent then obtains the action sequence that maximizes the reward function, determining the optimal trajectory of the USV. Specific steps include:

[0138] The SAC network structure includes an Actor network and a Critic network. The Actor network is responsible for fitting the stochastic policy function π. μ (a k |x s，k ), used to determine the state x s，k Select action a kThe Critic network consists of two parts: a V-network used to fit the state-value function. Used for state x s，k Evaluation is performed; Q-networks are used to fit the state-action value function. Used for state x s，k a below k Actions are evaluated. During training, both Q-networks are updated simultaneously, with the smaller Q-value selected for policy update in each iteration. The target-V network, on the other hand, is updated using a soft update mechanism to reduce the frequency of network parameter updates, thereby enhancing training stability.

[0139] The agent selects the output action at time k, and after interacting with the filtered environment, obtains the position x of the USV at time k+1. s，k+1 Then through the reward function r k The SAC algorithm evaluates the action by iteratively learning from historical experience, and finally obtains the optimal trajectory control strategy for USV path planning.

[0140] After the USV performs the optimal action, it moves to the corresponding position, obtains the measurement information of that position, and updates the motion state of the multi-target system.

[0141] It should be noted that the method of this embodiment can be executed by a single device, such as a computer or server. The method of this embodiment can also be applied to a distributed scenario, where multiple devices cooperate to complete the task. In such a distributed scenario, one of these devices may execute only one or more steps of the method of this embodiment, and the multiple devices will interact with each other to complete the method described. The path planning process of this embodiment is as follows: Figure 2 As shown.

[0142] It should be noted that the above description describes some embodiments of this disclosure. Other embodiments are within the scope of the appended claims. In some cases, it should be understood that the sequence number of each step in the above embodiments does not imply the order of execution; the execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention. The actions or steps recorded in the claims can be performed in a different order than that in the above embodiments and can still achieve the desired result. In addition, the processes depicted in the drawings do not necessarily require a specific or sequential order to achieve the desired result. In some embodiments, multitasking and parallel processing are also possible or may be advantageous.

[0143] Example 2

[0144] The effects of the present invention will be further explained below with reference to simulation experiments.

[0145] Three targets with different threat levels were positioned within the USV's monitoring range, each navigating along its own trajectory. Furthermore, two ocean currents were distributed within the area at different locations and speeds; this paper assumes that target movement is unaffected by these currents. The USV's primary task was to monitor the status of suspicious targets within the area, assess the threat level of each target, and rationally plan its navigation path under ocean current disturbances to effectively avoid target threats. Simultaneously, the USV needed to prioritize tracking the target with the highest threat level. The USV's initial position was set to [17500m, 0m]. T We conducted 100 independent Monte Carlo (MC) simulation experiments to statistically evaluate performance indicators, and analyzed the simulation results through comparative experiments.

[0146] To verify the effectiveness of the proposed method in tracking targets in a marine environment, four USV trajectory schemes were designed for comparison based on the above experimental scenario: Scheme 1 uses path planning based on the Particle Swarm Optimization (PSO) algorithm; Scheme 2 uses path planning based on the Deep Deterministic Policy Gradient (DDPG) algorithm; Scheme 3 uses USV path planning based on the Proximal Policy Optimization (PPO) algorithm; and Scheme 4 uses path planning based on the SAC algorithm. The threat levels of the four schemes were obtained from the threat paths in the DeepPath inference knowledge graph, and the reward functions are shown in Equation (25), with weights set as ω1 = 0.2, ω2 = 0.6, and ω3 = 0.2, respectively.

[0147] Figure 3 The results of USV path planning trained under four scenarios are presented. Under the influence of sensor noise, measurement source uncertainty, and environmental interference, the goal of USV path planning is to avoid ocean current disturbances and target threats while finding the optimal observation position for multi-target tracking. Therefore, the path planning method employed needs to continuously adjust the USV's position according to the criterion of maximizing the reward function to achieve optimal tracking performance. Figure 3 As shown, the three reinforcement learning-based path planning methods, SAC, PPO, and DDPG, all successfully avoided ocean current disturbances. However, the PSO-based path entered the ocean current region for a period of time. This is mainly because PSO lacks dynamic adaptability and therefore cannot react immediately to avoid ocean current disturbances. In contrast, among the reinforcement learning methods, SAC and PPO outperformed DDPG in path planning. Although DDPG avoided the threats of the target and ocean currents, its path gradually deviated from the target, which was detrimental to improving filtering accuracy. SAC and PPO, while effectively avoiding the threats of the target and ocean currents, gradually moved towards the target, prioritizing the tracking of targets with high threat levels, thus demonstrating better path planning capabilities.

[0148] Figure 4 This represents the statistical mean of the root mean square error (RMSE) for the four schemes in the MC experiment. Figure 4 (a), (b), and (c) show the trends of the errors of the three targets over the first 50 time steps. Simulation results show that the errors of the four methods—PSO, DDPG, PPO, and SAC—gradually decrease over time. Specifically, the error of PSO gradually approaches zero after the 30th step, while the errors of DDPG, PPO, and SAC decrease to zero after the 10th step. It can be seen that path planning based on reinforcement learning can reduce tracking errors more quickly, and its tracking performance is better in terms of real-time performance and speed compared to PSO. Although the above methods all have high accuracy in target estimation, comparing the overall trends of the curves shows that the overall error of the method proposed in this invention is lower than that of the other three methods. This fully verifies that the target tracking performance of the path planning method proposed in this invention is superior to other methods.

[0149] Figure 5 The target threat distribution density is displayed at 100 time points. Combined with target trajectory analysis, the threat distribution of target 2 exhibits a peak within the Threat_low range, with most data points concentrated in the low-threat area, indicating that the USV effectively avoided the threat of target 2 after path planning. In contrast, the threat distribution of target 1 is more widespread, with broad peaks near both Threat_low and Threat_high, where the density in the low-threat area is slightly higher than in the high-threat area. Conversely, target 3 shows a slightly higher density in the high-threat area than in the low-threat area, indicating that target 3 has the highest threat level. Therefore, in path planning, the USV should prioritize tracking target 3 to minimize potential threats.

[0150] Since USVs cannot effectively resist ocean currents from both the lateral and reverse directions, their course planning should avoid being perpendicular to or opposite to the current direction. When the USV's course aligns with the current direction, the current can effectively increase its power, thereby conserving energy. Figure 6 Box plots and normal distributions of the angles between the USV's heading and the ocean current direction in the MC experiment are shown. The results indicate that most angles are concentrated between 0° and 10°, which verifies the effectiveness of path planning in avoiding ocean current interference and, to some extent, reducing energy consumption by utilizing the propulsion of ocean currents.

[0151] Example 3

[0152] Based on the same inventive concept, corresponding to any of the above embodiments, this disclosure also provides an optimal trajectory planning system for unmanned surface vehicles in complex environments, including: an initialization module, a construction module, an inference module, an evaluation module, and a planning module; the initialization module is used to initialize the state of the target unmanned surface vehicle to obtain initial state information; the construction module is used to construct a system model based on the initial state information; the inference module is used to construct a threat-based knowledge graph based on the system model, and use a policy gradient reinforcement learning algorithm to infer the threat level to obtain inference results; the evaluation module is used to construct evaluation indicators based on the inference results; and the planning module is used to complete the optimal trajectory planning based on the evaluation indicators.

[0153] The following will, in conjunction with this embodiment, explain in detail how the present invention solves technical problems in real life.

[0154] First, the initialization module is used to initialize the target unmanned surface vehicle's state, obtaining initial state information. The process includes:

[0155] Let the multi-objective state set of t objectives at time k be... The sampling period is T = 1s, and the process noise matrix is ​​v. k Assume that m measurements are received at time k, and the multi-target measurement set Z k for Measurement noise matrix w k The sampling points are N=100, and the initial state information is obtained in this way.

[0156] The building module then constructs a system model based on the initial state information.

[0157] Based on the initial state information of the target, the state transition equation and measurement equation of the target are constructed; and a mathematical model of ocean current changes is constructed based on the numerical estimation of the Lamb and Navier-Stokes equations. The process includes:

[0158] (1) Modeling a multi-target tracking system

[0159] Let the state transition equation of the target at time k be:

[0160]

[0161] Where f(·) represents the state evolution mapping; v k This indicates process noise.

[0162] Assume the observation noise is w k The corresponding measurement equation is:

[0163]

[0164] Where, xs,k =[x s,k ,y s,k ] T The position of the USV (Unmanned Surface Vehicle) at time k is represented by h; the measurement map is represented by x. s,k The x-coordinate of USV at time k; y s,k The vertical coordinate of USV at time k is represented.

[0165] (2) Constructing an ocean current model

[0166]

[0167] Among them, u c This indicates the velocity of the ocean current in the y-direction; v c S represents the velocity of the ocean current in the x-direction. c =[x c ,y c ] T S represents the displacement vector of ocean currents. o (ΔT A )=[x o ,y o ] T ζ represents the vortex center vector; ζ represents the vortex intensity; l represents the vortex radius.

[0168] The reasoning module is based on the system model, constructs a threat-based knowledge graph, and uses a policy gradient reinforcement learning algorithm to reason about the threat level to obtain the reasoning result.

[0169] A threat assessment dataset is constructed, and then a supervised policy network is trained by exploring more effectively from a given positive sample subset. The knowledge in the positive sample subset encourages the PG's agent to explore entities that are beneficial to its policy learning. Then, based on the supervised policy, the agent learns actions that maximize the reward function to further explore paths to threatening entities from the threat assessment dataset, thus realizing the inference process regarding threat levels.

[0170] Specifically, a filtering environment based on multi-target tracking and deep reinforcement learning interaction is constructed. A threat-based knowledge graph is built by combining target and ocean current movement patterns, and reinforcement learning is used to infer threat levels. The process includes:

[0171] S301. Prior state of target t at time k and state prior covariance matrix Described as:

[0172]

[0173]

[0174] Among them, Fk|k-1 This is the state transition matrix; Let be the process noise covariance matrix.

[0175] After predicting the state in one step, select action a according to the policy. k USV from position x s，k-1 Move to x s，k The corresponding measurement information will be affected. Based on the measurement information after the USV moves, the posterior estimate of the target t's state can be obtained as follows:

[0176]

[0177] Among them, K k Represents the system gain matrix; Indicates the execution of action a k The i-th measurement The probability of association with target t, where i = 0 indicates that no measurement was performed. From target t; It indicates new information.

[0178] The posterior state covariance matrix of target t is:

[0179]

[0180] in, This represents the covariance matrix used to predict the uncertainty of the target state at time k based on the state estimate at time k-1. This indicates that USV is at position x s，k The new information covariance matrix; This represents the joint innovation of the target state estimated using all pseudo-measures under the associated probability.

[0181] After the S302.USV moves to the appropriate location, it obtains the data source for threat assessment using a knowledge graph and constructs a threat knowledge graph. Assume there are t targets and n ocean currents within the USV's monitoring area. Therefore, the set of head entity vectors at each time point can be represented as:

[0182]

[0183] in Let t represent the t-th target. This represents the nth ocean current.

[0184] The sets of relation vectors and tail entity vectors are as follows:

[0185]

[0186]

[0187] Where u represents the number of threat-related attributes, including target location, target velocity, target angle relative to the USV, and ocean current angle relative to the USV; This represents the u-th relation vector at time k. Let represent the u-th tail entity vector of target t at time k.

[0188] Based on the measurement information obtained from the radar, the position and angle of target t at time k are as follows:

[0189]

[0190]

[0191] Based on the state estimation information, the velocity tail entity of target t is:

[0192]

[0193] in, and These represent the target's lateral velocity and longitudinal velocity, respectively.

[0194] The angle between the ocean current and the USV can be calculated using the following formula:

[0195]

[0196] In summary, the set of positive samples is E. + ={H k L k P k To improve the accuracy and robustness of the algorithm, some triples are replaced with head or tail entities to generate negative samples. The knowledge base containing positive and negative samples forms a dataset E related to the target threat, denoted as:

[0197] E = E + ∪E - (45)

[0198] Among them, E + and E - These represent the positive sample set and the negative sample set, respectively.

[0199] S303. For each relation, a subset of all positive samples is used to learn a supervised policy. In each positive sample (e source e target In this paper, a bidirectional path constraint search (BFS) is used to determine the correct path between entities. Each path P contains a sequence of relations l1→l2→...→l n The parameter θ is updated using the Monte Carlo policy gradient method.

[0200] In supervised learning, a reward is given for each successful learning step. The approximate gradient update formula for the policy network, substituted into the correct path found, is as follows:

[0201]

[0202] Among them, s t ' represents the state of the input supervision policy at time t; J represents the path l t The sum of expected rewards; a represents the gradient with respect to the parameter θ; t ′ represents the action output by the supervision policy at time t; A represents the action space; l t ∈p.

[0203] Furthermore, through the pre-generated training set E tr The supervised policy network is then retrained using reinforcement learning to determine the inference path controlled by the reward function. In a knowledge graph environment, the agent's primary task is to determine whether the target entity e can be reached through a series of paths. target Therefore, the following accuracy reward function r was designed. GLOBAL :

[0204]

[0205] When an agent reaches the target entity after performing a series of actions, it receives a reward of +1; otherwise, it receives a reward of -1.

[0206] For each relation, the reasoning of an entity pair constitutes an act. Starting from the source node, the agent selects a relation to extend its reasoning path based on a random policy and the probability distribution of all relations. This random policy avoids the agent repeatedly reasoning in incorrect relations; upon failure, the agent remains on the original entity node, maintaining the same state. An act ends if the agent reaches the target entity within the maximum number of steps or fails to do so. After each act, the policy network is updated using the following gradients:

[0207]

[0208] Where J represents path l t The sum of expected rewards; a represents the gradient with respect to the parameter θ; t ' represents the action performed by the PG agent at time t (the relation node selected by the agent in the knowledge graph); s t ′ represents the state of the PG agent in the knowledge graph at time t.

[0209] To improve training efficiency, this invention uses an experience playback mechanism to store historical data in an experience pool for subsequent learning, and updates the experience pool every 100 times.

[0210] S4. Based on the reasoning results, construct evaluation indicators.

[0211] To meet the mission requirements of the USV, the reward function design needs to consider optimizing tracking performance and avoiding threats, prioritizing the tracking of targets with the highest threat. Specifically, based on state priors and covariance matrices, Cramer-Rao lower bounds for all targets are calculated to evaluate the performance of multi-target tracking. Threat levels are inferred in real time, and the distance between the USV and targets is calculated to determine the threat level of each target and ocean current, thereby avoiding target threats and ocean current disturbances, and prioritizing the tracking of high-threat targets. The specific steps are as follows:

[0212] Constructing evaluation metrics for path planning using multi-objective posterior Cramerlow lower bounds and threat information:

[0213] Assuming based on measurement The unbiased estimate of the state of target t is The following inequalities must be satisfied:

[0214]

[0215] in, This represents the true state of target t at time k; for Fisher Information Matrix (FIM).

[0216] The target at time k corresponds to It can be calculated as follows:

[0217]

[0218] in, The matrix representing the prior state information of the target; Indicates based on measurement Fisher's information matrix.

[0219] Let be the prior state information matrix of the target. The recursive formula for the information matrix can be expressed as:

[0220]

[0221]

[0222] in, This represents the target state at time k-1. Fisher's information matrix; This represents the target state at time k-1. arrive Fisher's information matrix; express arrive Fisher's information matrix; Represents the target state at time k. Fisher's information matrix.

[0223] The data information matrix representing the target is shown below:

[0224]

[0225] in, This indicates that the target state is given at time k. Under the conditions, observation The probability density function.

[0226] From the system's state equation and measurement equation, we can obtain:

[0227]

[0228]

[0229] in, This is the state transition matrix; This represents the inverse of the posterior information matrix at time k-1; The process noise covariance matrix; Represents the observation matrix; This represents the observation noise covariance matrix.

[0230] From equations (54) and (55), we can see that the lower bound of Cramer is... The expression is as follows:

[0231]

[0232] The diagonal elements of PCRLB provide a lower bound for the variance of each component in the estimated target state vector. In this embodiment, the tracking performance of the target motion state is evaluated by comparing the root mean square error of the target state estimation with the PCRLB. The closer the root mean square error is to the lower bound, the better the overall tracking performance. The expression is as follows:

[0233] r track =RMSE-PCRLB (57)

[0234] Where RMSE is the root mean square error of the target state estimation, and PCRLB is the posterior Cramero lower bound. Since threat levels in knowledge graph reasoning are categorized into high, medium, and low, the threat reward function defined in this embodiment is as follows to quantify threat levels:

[0235]

[0236] Where, η penalty =1 represents the penalty coefficient. When the threat level is `threat_low`, the threat reward is 0; otherwise, the threat reward is less than 0. Furthermore, USVs performing maritime surveillance missions need to collect information on targets with the highest threat level. Therefore, the path planning should prioritize tracking targets with the highest threat level. For this purpose, the distance between the USV and the target is used as the evaluation metric. The distance reward function is as follows:

[0237]

[0238] In summary, the following global reward function is constructed:

[0239] r k =R(a k )=ω1·r track +ω2·r threat +ω3·r dist (60)

[0240] S5. Based on the evaluation indicators, complete the optimal trajectory planning.

[0241] The SAC agent continuously interacts with the multi-target tracking filtering environment to obtain USV control data for training. The trained SAC agent then obtains the action sequence that maximizes the reward function, determining the optimal trajectory of the USV. Specific steps include:

[0242] The SAC network structure includes an Actor network and a Critic network. The Actor network is responsible for fitting the stochastic policy function π. μ (a k |x s，k ), used to determine the state x s，k Select action a k The Critic network consists of two parts: a V-network used to fit the state-value function. Used for state x s，k Evaluation is performed; Q-networks are used to fit the state-action value function. Used for state x s，k a below k Actions are evaluated. During training, both Q-networks are updated simultaneously, with the smaller Q-value selected for policy update in each iteration. The target-V network, on the other hand, is updated using a soft update mechanism to reduce the frequency of network parameter updates, thereby enhancing training stability.

[0243] The agent selects the output action at time k, and after interacting with the filtered environment, obtains the position x of the USV at time k+1. s，k+1 Then through the reward function r kThe SAC algorithm evaluates the action by iteratively learning from historical experience, and finally obtains the optimal trajectory control strategy for USV path planning.

[0244] After the USV performs the optimal action, it moves to the corresponding position, obtains the measurement information of that position, and updates the motion state of the multi-target system.

[0245] This disclosure is intended to cover all such substitutions, modifications, and variations that fall within the broad scope of the appended claims. Therefore, any omissions, modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this disclosure should be included within the scope of protection of this disclosure.

Claims

1. A method for optimal trajectory planning of unmanned surface vehicles in complex environments, characterized by the steps of Comprise: State initialization is carried out on the target to obtain initial state information; Based on the initial state information, a system model is constructed; A threat-based knowledge graph is constructed, and a policy gradient reinforcement learning algorithm is used to reason the threat degree to obtain threat degree information; The method for reasoning the threat degree using the policy gradient reinforcement learning algorithm comprises: constructing a threat evaluation data set, then training a supervised policy network by exploring from a given positive sample subset, and then further exploring the path of the threat entity from the threat evaluation data set based on the supervised policy, so that the agent learns the action that maximizes the reward function, and the reasoning process of the threat degree is realized; Based on the multi-target tracking process, a filtering environment interacting with deep reinforcement learning is constructed, a threat-based knowledge graph is constructed based on the movement situation of the target and the ocean current, and the threat degree is reasoned using reinforcement learning, and the steps comprise: S301. State prior of target t at time k and state prior covariance matrix is described as: where F k|k-1 is the state transition matrix; is the process noise covariance matrix; After one-step prediction of the state, the action a is selected according to the policy k , the USV moves from position x s，k-1 to x s,k , and the posterior estimate of the state of the target t can be obtained from the measurement information after the USV moves as follows: where K k represents the system gain matrix; represents the execution of action a k the ith measurement the association probability with target t, when i = 0, represents no measurement from target t; represents the innovation; The posterior state covariance matrix of the target t is: wherein, represents a covariance matrix predicting the uncertainty of the target state at time k based on the state estimate at time k-1; represents the innovation covariance matrix of the USV at position x s,k ; represents the joint innovation of the target state estimated with all pseudo-measurements under the association probability. S302. After the USV moves to the corresponding position, the knowledge graph evaluation threat data source is obtained, and a threat knowledge graph is constructed; it is assumed that there are t targets and n ocean currents in the USV monitoring area, therefore, the head entity vector set at time k is represented as: wherein denotes the t-th target, denotes the n-th ocean current; The relationship vector and the tail entity vector set are: where u represents the number of threat-related attributes, including target position, target velocity, target angle relative to the USV, and ocean current angle relative to the USV; represents the u-th relationship vector of the target t at time k, represents the u-th tail entity vector of the target t at time k. According to the measurement information obtained by the radar, the position and angle of the target t at time k are as follows: According to the state estimation information, the velocity tail entity of the target t is: wherein, and Vxand Vydenote the lateral and longitudinal velocities of the target, respectively; The angle between the ocean current and the USV can be calculated by the following formula: The positive sample set is E + = {H k , L k , P k}; the head entity or the tail entity of the part of the triple is replaced to generate a negative sample, and a knowledge base containing the positive sample and the negative sample constitutes a data set E related to the target threat, and is represented as: E = E + ∪E - wherein E + and E - respectively represent the positive sample set and the negative sample set; Based on the multi-target posterior Cramer-Lo lower bound and the threat degree information, an evaluation index is constructed; the method for constructing the evaluation index comprises: calculating the Cramer-Lo lower bound of all targets based on the state prior and the covariance matrix, which is used to evaluate the performance of multi-target tracking; then, the threat degree is reasoned in real time, and the distance between the USV and the target is calculated to determine the threat level of each target and the ocean current, so as to avoid target threats and ocean current disturbances, and preferentially track targets with high threat; An optimal decision of the USV path planning is made based on the SAC deep reinforcement learning algorithm, and optimal trajectory planning is completed.

2. The method of claim 1, wherein, The method of initializing the state includes setting a multi-target state set of t targets at time k as The sampling period is T = 1 s, the process noise matrix v k , assuming that m measurements are received at time k, a multi-target measurement set Z k is The measurement noise matrix w k , and the sampling points are N = 100.

3. The method of claim 2, wherein, The system model comprises: a state transition equation of the target, a measurement equation of the target, and an ocean current model; wherein the state transition equation of the target comprises: where f(·) denotes the state evolution map; v k denotes the process noise; The measurement equation of the target comprises: wherein x s,k = [x s,k , y s,k ] T represents the position of the USV at time k; h represents the measurement map; x s,k represents the x-coordinate of the USV at time k; and y s,k represents the y-coordinate of the USV at time k. The ocean current model comprises: where u c represents the velocity of the ocean current in the y direction; v c represents the velocity of the ocean current in the x direction; S c = [x c , y c ] T represents the displacement vector of the ocean current; S o = [x o , y o ] T represents the vortex core vector; ζ represents the vortex strength; and l represents the vortex radius.

4. The method of claim 1, wherein, The method for completing the optimal trajectory planning comprises: the SAC agent constantly interacts with the multi-target tracking filtering environment to obtain the control data of the unmanned surface vehicle to train the agent, and finally the action sequence that maximizes the reward function is obtained through the trained SAC agent, and the optimal trajectory of the unmanned surface vehicle is decided.

5. An optimal trajectory planning system for unmanned surface vehicles in complex environments, the system being configured to implement the method of any one of claims 1 to 4, characterized in that, Comprise: Initialization module, construction module, reasoning module, evaluation module and planning module; The initialization module is used for state initialization of the target to obtain initial state information; The construction module is used for constructing a system model based on the initial state information; The reasoning module is used for constructing a threat-based knowledge graph based on the system model, and reasoning the threat degree using a policy gradient reinforcement learning algorithm to obtain a reasoning result; The evaluation module is used for constructing an evaluation index based on the reasoning result; The planning module is configured to complete optimal trajectory planning based on the evaluation index.

Images