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

Abstract

The invention discloses an optimal trajectory planning method and system for an unmanned surface vehicle (USV) in a complex environment, and the method comprises the steps: carrying out the state initialization of a target, and obtaining the initial state information; constructing a system model based on the initial state information; constructing a knowledge graph based on threats, and reasoning to obtain threat degree information; combining the multi-target posterior Cramer-Rao lower bound and threat degree information to construct an evaluation index; and based on the evaluation indexes, completing optimal trajectory planning. According to the method, a knowledge graph based on marine threats is constructed in combination with a target and an ocean current movement situation. Threat semantic information in the atlas is reasoned through a reinforcement learning method, the defect that a traditional threat assessment method is lack of self-learning ability for threat features is effectively overcome, and the correlation between assessment indexes can be deeply mined. According to the method, the target tracking effect can be optimized while target threat and ocean current disturbance are effectively avoided.

Description

Technical Field

[0001] The present invention relates to the technical field of radar signal processing technology, and in particular to an optimal trajectory planning method and system for an unmanned surface vehicle in a complex environment. Background Art

[0002] With the continuous advancement of marine resource utilization technology, countries have gradually increased their attention to maritime security, making the ocean the strategic focus of modern battlefields. USV (unmanned surface vehicle) is widely used in maritime reconnaissance and early warning, target monitoring and anti-submarine tasks due to its advantages such as concealment, scalability and flexibility. However, due to the complex and changeable marine environment, the navigation and control of USV are easily affected by external interference such as time-varying sea breezes, ocean currents and waves. Therefore, effectively ensuring its 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 tasks and maintaining maritime security. With the rapid development of artificial intelligence technology, unmanned surface navigation systems not only have information perception capabilities, but also have logical reasoning, inference decision-making and adaptive capabilities. In particular, how to achieve intelligent decision-making in complex environments has become a research focus.

[0003] In the path planning problem of USV, reinforcement learning (RL), as a trial-and-error learning method, learns the optimal strategy 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 dimensionality explosion problems when faced with 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 ability of reinforcement learning, providing a new solution to path planning problems in complex environments.

[0004] Traditional threat assessment methods usually rely on expert experience to determine threat indicators and build models. However, these methods involve complex theoretical knowledge, and the process of building and adjusting models is cumbersome. At the same time, they have high requirements for data quality and integrity. Poor data quality or serious data loss may affect the accuracy and credibility of the assessment results. Therefore, how to establish a more dynamic and real-time intelligent threat assessment method has become the main direction of current threat assessment research. In recent years, knowledge graphs have become the focus of research because they can 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] In order to solve the above technical problems, the present invention provides an optimal trajectory planning method for an unmanned surface vehicle in a complex environment, the steps comprising:

[0006] Initializing the state of the target unmanned surface vehicle to obtain initial state information;

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

[0008] Based on the system model, a threat-based knowledge graph is constructed, and the threat degree is inferred using a policy gradient reinforcement learning algorithm to obtain an inference result;

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

[0010] Based on the evaluation index, the optimal trajectory planning is completed.

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

[0012] Preferably, the system model includes: a state transfer equation of the target, a measurement equation of the target and an ocean current model; wherein the state transfer equation of the target includes:

[0013]

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

[0015] The measurement equation of the target includes:

[0016]

[0017] Among them, 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 horizontal coordinate of USV at time k; y s,k represents the ordinate of USV at time k;

[0018] The ocean current model includes:

[0019]

[0020] Among them, u c represents the speed of the ocean current in the y direction; v c represents the speed of the ocean current in the x direction; Sc =[x c ,y c ] T represents the displacement vector of the ocean current; S 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 of using a policy gradient reinforcement learning algorithm to reason about the threat level includes: constructing a threat assessment data set, then training a supervision policy network by exploring from a given subset of positive samples, and then, based on the supervision strategy, enabling the agent to learn actions that maximize the reward function to further explore the path of the threat entity from the threat assessment data set, thereby realizing the reasoning process of the threat level.

[0022] Preferably, the method for constructing the evaluation index includes: calculating the Cramer-Rao 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: utilizing the SAC agent to continuously interact with the multi-target tracking 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 decide the optimal trajectory of the unmanned surface vehicle.

[0024] The present invention also provides an optimal trajectory planning system for unmanned surface vehicles in complex environments, the system being used to implement the above method, comprising: 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 to 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 use a policy gradient reinforcement learning algorithm to reason about the threat level to obtain a reasoning result;

[0028] The evaluation module is used to construct an evaluation index based on the reasoning result;

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

[0030] Compared with the prior art, the present invention has the following beneficial effects:

[0031] An intelligent decision-making method based on deep reinforcement learning is proposed for USV path planning in complex environments. In this method, a knowledge graph based on ocean threats is constructed by combining the target and ocean current movement situation. The threat semantic information in the graph is inferred by reinforcement learning method, which effectively solves the shortcomings of traditional threat assessment methods that lack the self-learning ability of threat characteristics and can deeply explore the correlation between evaluation indicators. In addition, the threat information and the posterior Cramer-Rao lower bound of multiple targets are used as the joint reward function of USV path planning, which effectively avoids target threats and ocean current disturbances while optimizing the tracking effect of the target. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] In order to more clearly illustrate the technical solution of the present invention, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0033] Figure 1 A schematic diagram of a method flow chart of an embodiment of the present invention;

[0034] Figure 2 This is a flow chart of path planning according to an embodiment of the present invention;

[0035] Figure 3 A schematic diagram of a path planning of a USV monitoring target in a complex marine environment according to an embodiment of the present invention;

[0036] Figure 4 Schematic diagram of the root mean square error of the USV tracking target of the RMSE of the three targets in the embodiment of the present invention; wherein, Figure 4 (a), (b), and (c) are the time-varying trends of the errors of the three targets in the first 50 time steps;

[0037] Figure 5 A schematic diagram of the threat distribution density of each target in an embodiment of the present invention;

[0038] Figure 6 Schematic diagram of the angle between the USV heading and the ocean current direction according to an embodiment of the present invention. DETAILED DESCRIPTION

[0039] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0040] It should be noted that, unless otherwise defined, the technical terms or scientific terms used in the embodiments of the present disclosure should be understood by people with ordinary skills in the field to which the present disclosure belongs. The "first", "second" and similar words used in the embodiments of the present disclosure do not indicate any order, quantity or importance, but are only used to distinguish different components. "Including" or "comprising" and similar words mean that the elements or objects appearing before the word cover the elements or objects listed after the word and their equivalents, without excluding other elements or objects. "Connect" or "connected" and similar words are not limited to physical or mechanical connections, but can include electrical connections, whether direct or indirect. "Up", "down", "left", "right" and the like are only used 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 the present invention are explained:

[0042] USV: Unmanned Surface Vehicle.

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

[0044] PCRLB: Posterior Cramér-Rao Lower Bound, which is a tool to measure the estimation accuracy of nonlinear filtering problems. It can provide a lower bound for the target state estimation error at the next moment, so it is often used as an evaluation criterion for moving target tracking errors.

[0045] PG: stands for "Policy Gradient", which is a reinforcement learning algorithm. Policy gradient methods directly learn policies by optimizing the parameters of a policy function, which defines the probability of choosing each possible action in a given state. This method calculates the gradient and updates the policy parameters to enhance the probability of actions that lead to higher cumulative rewards.

[0046] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0047] Embodiment 1

[0048] As can be seen from the background technology, traditional threat assessment methods usually rely on expert experience to determine threat indicators and build models. However, these methods involve complex theoretical knowledge, and the process of building and adjusting models is cumbersome. At the same time, they have high requirements for data quality and integrity. Poor data quality or serious data loss may affect the accuracy and credibility of the assessment results. Therefore, how to establish a more dynamic and real-time intelligent threat assessment method has become the main direction of current threat assessment research. In recent years, knowledge graphs have become the focus of research because of 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] The embodiment of the present invention provides an optimal trajectory planning method for an unmanned surface vehicle in a complex environment. Figure 1 As shown, the steps include:

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

[0051] The multi-objective state set of t objectives at k time is set as The sampling period is T = 1s, and the process noise matrix v k , assuming that m measurements are received at time k, the multi-target measurement set Z k for The measurement noise matrix w k , the sampling point is N=100, so as to obtain the initial state information.

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

[0053] The state transfer equation and measurement equation of the target are constructed based on the initial state information of the target; and the mathematical model of ocean current changes is constructed based on the numerical estimation of Lamb and Navier-Stokes equations. The steps include:

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

[0055] Assume that the state transfer equation of the target at time k is:

[0056]

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

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

[0059]

[0060] Among them, x s,k =[x s,k ,y s,k ] T represents the position of USV (unmanned surface vehicle) at time k; h represents the measurement mapping; x s,k represents the horizontal coordinate of USV at time k; y s,k Represents the ordinate of USV at time k.

[0061] (2) Building an ocean current model

[0062]

[0063] Among them, u c represents the speed of the ocean current in the y direction; v c represents the speed 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 (Δ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, a threat-based knowledge graph is constructed, and the policy gradient reinforcement learning algorithm is used to infer the threat degree to obtain the inference result.

[0065] Construct a threat assessment dataset, and then train the supervision policy network by more effectively exploring from a given subset of positive samples. The knowledge in the positive sample subset encourages the PG agent to explore entities that are conducive to its policy learning. Then, based on the supervision strategy, the agent learns actions that maximize the reward function to further explore the path of threat entities from the threat assessment dataset, realizing the reasoning process of threat degree.

[0066] Specifically, a filtering environment that interacts with deep reinforcement learning is constructed based on the multi-target tracking process, a threat-based knowledge graph is constructed based on the target and ocean current movement situation, and reinforcement learning is used to infer the threat level. The steps include:

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

[0068]

[0069]

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

[0071] After the state is predicted one step further, action a is selected according to the strategy k , USV from position x s，k-1 Move to x s，k , the corresponding measurement information will be affected. According to the measurement information after the USV moves, the state posterior estimate of the target t can be obtained as:

[0072]

[0073] Among them, K k represents the system gain matrix; Indicates execution of action a k After the i-th measurement The probability of association with target t, when i = 0, it means there is no measurement from target t; Indicates new information.

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

[0075]

[0076] in, represents the covariance matrix for predicting the uncertainty of the target state at time k based on the state estimate at time k-1; Indicates that the USV is at position x s，k The innovation covariance matrix on ; represents the joint innovation of estimating the target state using all pseudo measurements under the associated probability.

[0077] S302. After the USV moves to the corresponding position, the data source of the knowledge graph for threat assessment is obtained, and the threat knowledge graph is constructed. Assume that there are t targets and n ocean currents in the USV monitoring area. Therefore, the moment header entity vector set can be expressed as:

[0078]

[0079] in represents the tth target, Represents the nth ocean current.

[0080] The relationship vector and tail entity vector sets are:

[0081]

[0082]

[0083] Among them, u represents the number of attributes related to the threat, including the target position, target speed, the angle of the target relative to the USV, and the angle of the ocean current to the USV; represents the u-th relationship vector at time k, Represents the u-th tail entity vector of target t at time k.

[0084] From the measurement information obtained by the radar, we can know that the position and angle tail entity of target t at time k are as follows:

[0085]

[0086]

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

[0088]

[0089] in, and Represent the lateral and longitudinal velocities of the target respectively.

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

[0091]

[0092] In summary, the positive sample set is E + ={H k , L k , P k In order to improve the accuracy and robustness of the algorithm, the head entity or tail entity of some triples is replaced to generate negative samples. The knowledge base containing positive and negative samples constitutes the data set E related to the target threat, which is expressed as:

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

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

[0095] S303. For each relationship, a subset of all positive samples is used to learn the supervision strategy. source , e target ) uses bidirectional path constraint search (BFS) to determine the correct path between entities. Each path P contains a relation sequence l 1 → 2 →...→l n , and update the parameters θ through the Monte Carlo policy gradient method.

[0096] In supervised learning, each successful step of learning will receive a reward. Substituting the found correct path into the approximate gradient update formula of the policy network is as follows:

[0097]

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

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

[0100]

[0101] Among them, when the agent reaches the target entity after performing a series of actions, it receives a reward of +1, otherwise it is -1.

[0102] For each relationship, the reasoning of an entity pair is an episode. Starting from the source node, the agent selects a relationship to expand its reasoning path according to the random strategy and the probability distribution of all relationships. This random strategy prevents the agent from repeating reasoning in the wrong relationship. After failure, the agent will stay at the original entity node and maintain the same state. If the agent reaches the target entity within the maximum number of steps or fails to reach it, the episode ends. The policy network behind each episode is updated using the following gradient:

[0103]

[0104] Where J represents the path l t The expected sum of rewards; represents the gradient of the parameter θ; a 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] In order to improve the training efficiency, the present invention uses an experience replay mechanism to store historical data in an experience pool for subsequent learning, and updates the experience pool every 100 times.

[0106] S4. Construct evaluation indicators based on the reasoning results.

[0107] In order to meet the mission requirements of USV, the reward function design needs to consider optimizing tracking effects and avoiding threats and giving priority to tracking the targets with the highest threat. Specifically, the Cramer-Rao lower bound of all targets is calculated based on the state prior and covariance matrix to evaluate the performance of multi-target tracking. The threat level is inferred in real time and the distance between the USV and the target is calculated to determine the threat level of each target and ocean current, so as to avoid target threats and ocean current disturbances and give priority to tracking targets with high threats. The specific steps are as follows:

[0108] The evaluation index of path planning is constructed using the multi-objective posterior Cramer-Rao lower bound and threat information:

[0109] Assumptions based on measurements The unbiased estimate of the state of the target t is Satisfies the following inequality:

[0110]

[0111] in, represents the true state of target t at time k; for The Fisher information matrix (FIM) of .

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

[0113]

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

[0115] is the state prior information matrix of the target, and the recursive formula of the information matrix can be expressed as:

[0116]

[0117]

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

[0119] The data information matrix representing the target is expressed as follows:

[0120]

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

[0122] From the state equation and measurement equation of the system, we can get:

[0123]

[0124]

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

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

[0127]

[0128] The diagonal elements of PCRLB provide the lower bound of the variance of each component in the estimated target state vector. This embodiment evaluates the tracking effect of the target motion state by comparing the difference between the root mean square error of the target state estimate and PCRLB. The closer the root mean square error is to the lower bound, the better the overall tracking performance is. The expression is as follows:

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

[0130] Among them, RMSE is the root mean square error of the target state estimation, and PCRLB is the posterior Cramer-Rao lower bound. Since the threat level in knowledge graph reasoning is divided into three categories: high, medium, and low. To quantify the threat level, the threat reward function defined in this embodiment is as follows:

[0131]

[0132] Among them, η penalty =1 is the penalty coefficient. When the threat level is threat_low, the threat reward is 0, otherwise, the threat reward is less than 0. In addition, USV needs to collect information about the target with the highest threat level when performing maritime monitoring tasks. Therefore, the target with the highest threat level should be tracked first in path planning. To this end, the distance between the USV and the target is used as an evaluation indicator. The distance reward function is as follows:

[0133]

[0134] To sum up, 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. Complete the optimal trajectory planning based on the evaluation indicators.

[0137] The SAC agent is used to continuously interact with the multi-target tracking filter environment to obtain the control data of the USV to train the agent. The trained SAC agent finally obtains the action sequence that maximizes the reward function and determines the optimal trajectory of the USV. The specific steps include:

[0138] The network structure of SAC includes Actor network and Critic network. Actor network is responsible for fitting the random strategy function π μ (a k |x s，k ), used to calculate the state x s，kSelect action a k ; The Critic network consists of two parts: the V network is used to fit the state value function For the state x s，k Evaluation; Q network is used to fit the state-action value function For the state x s，k A k The action is evaluated. During the training process, the two Q networks are updated simultaneously, and a smaller Q value is selected for policy update in each iteration. The target-V network is updated through a soft update mechanism to reduce the update frequency of network parameters, thereby enhancing the stability of training.

[0139] The agent selects the output action at time k and interacts with the filtering environment to obtain the position x of the USV at time k+1 s，k+1 , and then through the reward function r k The action is evaluated, and the SAC algorithm learns historical experience through iteration, and finally obtains the optimal trajectory control strategy for USV path planning.

[0140] After executing the optimal action, the USV moves to the corresponding position, obtains the measurement information of the position, and updates the motion status of multiple targets.

[0141] It should be noted that the method of the embodiment of the present disclosure can be executed by a single device, such as a computer or server. The method of the present embodiment can also be applied in a distributed scenario and completed by multiple devices cooperating with each other. In the case of such a distributed scenario, one of the multiple devices can only execute one or more steps of the method of the embodiment of the present disclosure, and the multiple devices will interact with each other to complete the method. The path planning process of this embodiment is as follows Figure 2 shown.

[0142] It should be noted that some embodiments of the present disclosure are described above. Other embodiments are within the scope of the attached claims. In some cases, it should be understood that the size of the serial number of each step in the above embodiment does not mean the order of execution, and 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 embodiment of the present invention. The actions or steps recorded in the claims can be performed in an order different from that in the above embodiment and still achieve the desired results. In addition, the process depicted in the accompanying drawings does not necessarily require the specific order or continuous order shown to achieve the desired results. In some embodiments, multitasking and parallel processing are also possible or may be advantageous.

[0143] Embodiment 2

[0144] The effect of the present invention is further described below in conjunction with simulation experiments.

[0145] Three targets with different threat levels are set within the monitoring range of the USV, and they sail along their respective trajectories. In addition, two ocean currents are distributed in the area at different positions and speeds. This paper assumes that the target movement is not affected by the ocean currents. The main task of the USV is to monitor the status of suspicious targets in the area, evaluate the threat level of each target, and reasonably plan the navigation path under the condition of ocean current disturbance to effectively avoid target threats. At the same time, the USV needs to give priority to tracking the target with the highest threat level. The initial position of the USV is set to [17500m, 0m] T , 100 independent Monte Carlo (MC) simulation experiments were conducted to calculate the statistics of performance evaluation indicators, and the simulation results were analyzed through comparative experiments.

[0146] In order to verify the effectiveness of the proposed method in tracking targets in a marine environment, four USV motion trajectory schemes were designed for comparison based on the above experimental scenarios: Scheme 1 adopts path planning based on the particle swarm optimization (PSO) algorithm, and Scheme 2 is path planning based on the deep deterministic policy gradient (DDPG) algorithm. Scheme 3 is USV path planning based on the proximal policy optimization (PPO) algorithm. Scheme 4 is path planning based on the SAC algorithm. The threat levels of the four schemes are obtained through the threat paths in the DeepPath reasoning knowledge graph. The reward function is shown in Formula (25), and its weights are set to ω. 1 =0.2,ω 2 =0.6,ω 3 =0.2.

[0147] Figure 3 The USV path planning results of four training schemes under ocean current disturbance are shown. Under the influence of sensor noise, measurement source uncertainty and environmental interference, the purpose of USV path planning is to avoid the threat of ocean current disturbance and target, and find the best observation position for multi-target tracking. Therefore, the path planning method used needs to continuously adjust the position of the USV according to the criterion of maximizing the reward function to achieve the best tracking performance. Figure 3As shown in the figure, the three reinforcement learning-based path planning methods SAC, PPO and DDPG all successfully avoided the disturbance of ocean currents. The path based on PSO entered the ocean current area for a period of time. This is mainly because PSO lacks dynamic adaptability, and cannot respond immediately to avoid the disturbance of ocean currents. In contrast, among the reinforcement learning methods, SAC and PPO performed better than DDPG in path planning. Although DDPG avoided the threat of targets and ocean currents, its path gradually deviated from the target, which is not conducive to the improvement of filtering accuracy. While effectively avoiding the threat of targets and ocean currents, SAC and PPO gradually moved towards the target direction, giving priority to tracking targets with high threat levels, thus showing better path planning capabilities.

[0148] Figure 4 It is the statistical mean of the target root mean square error (RMSE) of the four schemes in the MC experiment. Figure 4 (a), (b), and (c) are the trends of the errors of the three targets over time in the first 50 time steps. The 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 0 after the 30th step, while the error of DDPG, PPO, and SAC decreases to 0 after the 10th step. It can be seen that the path planning based on reinforcement learning can reduce the tracking error faster, and its tracking effect has better real-time and rapidity than PSO. Although the above methods all have high accuracy in target estimation, it can be seen from the overall trend of the comparison curve that the overall error of the method proposed in the present invention is lower than that of the three methods. It is fully verified that the target tracking performance of the path planning method proposed in the present invention is better than other methods.

[0149] Figure 5 The target threat distribution density at 100 moments is shown. Combined with the target motion trajectory analysis, the threat distribution of target 2 is peaked in the Threat_low range, and most of the data points are concentrated in the low threat area, indicating that the USV effectively avoids the threat of target 2 after path planning. In contrast, the threat distribution of target 1 is more extensive, with wide peaks near Threat_low and Threat_high, where the density in the low threat area is slightly higher than that in the high threat area. On the contrary, target 3 shows that the density in the high threat area is slightly higher than that in the low threat area, indicating that target 3 has the highest threat level. Therefore, in path planning, the USV should give priority to tracking target 3 to minimize potential threats.

[0150] Since USV cannot effectively resist the ocean currents from lateral and reverse impacts, the USV's path planning should avoid its heading being perpendicular and opposite to the ocean current direction. When the USV's heading is consistent with the ocean current direction, the ocean current can effectively increase its power, thereby saving energy. Figure 6 The box plot and normal curve distribution of the angle between the USV heading and the ocean current direction in the MC experiment. The results show that most of the angles are concentrated between 0° and 10°, which verifies the effectiveness of path planning in avoiding ocean current interference and using the driving force of ocean currents to reduce its own energy loss to a certain extent.

[0151] Embodiment 3

[0152] Based on the same inventive concept, corresponding to any of the above-mentioned embodiment methods, the present disclosure also provides an optimal trajectory planning system for an unmanned surface vehicle in a complex environment, including: an initialization module, a construction module, a reasoning 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 build a system model based on the initial state information; the reasoning module is used to build a threat-based knowledge graph based on the system model, and use a policy gradient reinforcement learning algorithm to reason about the threat degree to obtain a reasoning result; the evaluation module is used to build an evaluation index based on the reasoning result; the planning module is used to complete the optimal trajectory planning based on the evaluation index.

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

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

[0155] The multi-objective state set of t objectives at k time is set as The sampling period is T = 1s, and the process noise matrix v k , assuming that m measurements are received at time k, the multi-target measurement set Z k for The measurement noise matrix w k , the sampling point is N=100, so as to obtain the initial state information.

[0156] The construction module then builds the system model based on the initial state information.

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

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

[0159] Assume that the state transfer equation of the target at time k is:

[0160]

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

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

[0163]

[0164] Among them, x s,k =[x s,k ,y s,k ] T represents the position of USV (unmanned surface vehicle) at time k; h represents the measurement mapping; x s,k represents the horizontal coordinate of USV at time k; y s,k Represents the ordinate of USV at time k.

[0165] (2) Building an ocean current model

[0166]

[0167] Among them, u c represents the speed of the ocean current in the y direction; v c represents the speed 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 (Δ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 builds a threat-based knowledge graph based on the system model, and uses the policy gradient reinforcement learning algorithm to reason about the threat level to obtain the reasoning result.

[0169] Construct a threat assessment dataset, and then train the supervision policy network by more effectively exploring from a given subset of positive samples. The knowledge in the positive sample subset encourages the PG agent to explore entities that are conducive to its policy learning. Then, based on the supervision strategy, the agent learns actions that maximize the reward function to further explore the path of threat entities from the threat assessment dataset, realizing the reasoning process of threat degree.

[0170] Specifically, a filtering environment that interacts with deep reinforcement learning is built based on the multi-target tracking process, a threat-based knowledge graph is built based on the target and ocean current movement situation, and reinforcement learning is used to reason about the threat level. The process includes:

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

[0172]

[0173]

[0174] Among them, F k|k-1 is the state transfer matrix; is the process noise covariance matrix.

[0175] After the state is predicted one step further, action a is selected according to the strategy k , USV from position x s，k-1 Move to x s，k , the corresponding measurement information will be affected. According to the measurement information after the USV moves, the state posterior estimate of the target t can be obtained as:

[0176]

[0177] Among them, K k represents the system gain matrix; Indicates execution of action a k After the i-th measurement The probability of association with target t, when i = 0, it means there is no measurement from target t; Indicates new information.

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

[0179]

[0180] in, represents the covariance matrix for predicting the uncertainty of the target state at time k based on the state estimate at time k-1; Indicates that the USV is at position x s，k The innovation covariance matrix on ; represents the joint innovation of estimating the target state using all pseudo measurements under the associated probability.

[0181] S302. After the USV moves to the corresponding position, the data source of the knowledge graph for threat assessment is obtained, and the threat knowledge graph is constructed. Assume that there are t targets and n ocean currents in the USV monitoring area. Therefore, the moment header entity vector set can be expressed as:

[0182]

[0183] in represents the tth target, Represents the nth ocean current.

[0184] The relationship vector and tail entity vector sets are:

[0185]

[0186]

[0187] Among them, u represents the number of attributes related to the threat, including the target position, target speed, the angle of the target relative to the USV, and the angle of the ocean current to the USV; represents the u-th relationship vector at time k, Represents the u-th tail entity vector of target t at time k.

[0188] From the measurement information obtained by the radar, we can know that the position and angle tail entity of target t at time k are as follows:

[0189]

[0190]

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

[0192]

[0193] in, and Represent the lateral and longitudinal velocities of the target respectively.

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

[0195]

[0196] In summary, the positive sample set is E + ={H k , L k , P k In order to improve the accuracy and robustness of the algorithm, the head entity or tail entity of some triples is replaced to generate negative samples. The knowledge base containing positive and negative samples constitutes the data set E related to the target threat, which is expressed as:

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

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

[0199] S303. For each relationship, a subset of all positive samples is used to learn the supervision strategy. source , e target ) uses bidirectional path constraint search (BFS) to determine the correct path between entities. Each path P contains a relation sequence l 1 → 2 →...→l n , and update the parameters θ through the Monte Carlo policy gradient method.

[0200] In supervised learning, each successful step of learning will receive a reward. Substituting the found correct path into the approximate gradient update formula of the policy network is as follows:

[0201]

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

[0203] Furthermore, by using the pre-generated training set E tr Reinforcement learning is used to retrain the supervised policy network to determine the reasoning path controlled by the reward function. In the knowledge graph environment, the first task of the agent is to determine whether it can reach the target entity e through a series of paths. target Therefore, the following accuracy reward function r is designed GLOBAL :

[0204]

[0205] Among them, when the agent reaches the target entity after performing a series of actions, it receives a reward of +1, otherwise it is -1.

[0206] For each relationship, the reasoning of an entity pair is an episode. Starting from the source node, the agent selects a relationship to expand its reasoning path according to the random strategy and the probability distribution of all relationships. This random strategy prevents the agent from repeating reasoning in the wrong relationship. After failure, the agent will stay at the original entity node and maintain the same state. If the agent reaches the target entity within the maximum number of steps or fails to reach it, the episode ends. The policy network behind each episode is updated using the following gradient:

[0207]

[0208] Where J represents the path l t The expected sum of rewards; represents the gradient of the parameter θ; a 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] In order to improve the training efficiency, the present invention uses an experience replay mechanism to store historical data in an experience pool for subsequent learning, and updates the experience pool every 100 times.

[0210] S4. Construct evaluation indicators based on the reasoning results.

[0211] In order to meet the mission requirements of USV, the reward function design needs to consider optimizing tracking effects and avoiding threats and giving priority to tracking the targets with the highest threat. Specifically, the Cramer-Rao lower bound of all targets is calculated based on the state prior and covariance matrix to evaluate the performance of multi-target tracking. The threat level is inferred in real time and the distance between the USV and the target is calculated to determine the threat level of each target and ocean current, so as to avoid target threats and ocean current disturbances and give priority to tracking targets with high threats. The specific steps are as follows:

[0212] The evaluation index of path planning is constructed using the multi-objective posterior Cramer-Rao lower bound and threat information:

[0213] Assumptions based on measurements The unbiased estimate of the state of the target t is Satisfies the following inequality:

[0214]

[0215] in, represents the true state of target t at time k; for The Fisher information matrix (FIM) of .

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

[0217]

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

[0219] is the state prior information matrix of the target, and the recursive formula of the information matrix can be expressed as:

[0220]

[0221]

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

[0223] The data information matrix representing the target is expressed as follows:

[0224]

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

[0226] From the state equation and measurement equation of the system, we can get:

[0227]

[0228]

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

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

[0231]

[0232] The diagonal elements of PCRLB provide the lower bound of the variance of each component in the estimated target state vector. This embodiment evaluates the tracking effect of the target motion state by comparing the difference between the root mean square error of the target state estimate and PCRLB. The closer the root mean square error is to the lower bound, the better the overall tracking performance is. The expression is as follows:

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

[0234] Among them, RMSE is the root mean square error of the target state estimation, and PCRLB is the posterior Cramer-Rao lower bound. Since the threat level in knowledge graph reasoning is divided into three categories: high, medium, and low. To quantify the threat level, the threat reward function defined in this embodiment is as follows:

[0235]

[0236] Among them, η penalty =1 is the penalty coefficient. When the threat level is threat_low, the threat reward is 0, otherwise, the threat reward is less than 0. In addition, USV needs to collect information about the target with the highest threat level when performing maritime monitoring tasks. Therefore, the target with the highest threat level should be tracked first in path planning. To this end, the distance between the USV and the target is used as an evaluation indicator. The distance reward function is as follows:

[0237]

[0238] To sum up, 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. Complete the optimal trajectory planning based on the evaluation indicators.

[0241] The SAC agent is used to continuously interact with the multi-target tracking filter environment to obtain the control data of the USV to train the agent. The trained SAC agent finally obtains the action sequence that maximizes the reward function and determines the optimal trajectory of the USV. The specific steps include:

[0242] The network structure of SAC includes Actor network and Critic network. Actor network is responsible for fitting the random strategy function π μ (a k |x s，k ), used to calculate the state x s，kSelect action a k ; The Critic network consists of two parts: the V network is used to fit the state value function For the state x s，k Evaluation; Q network is used to fit the state-action value function For the state x s，k A k The action is evaluated. During the training process, the two Q networks are updated simultaneously, and a smaller Q value is selected for policy update in each iteration. The target-V network is updated through a soft update mechanism to reduce the update frequency of network parameters, thereby enhancing the stability of training.

[0243] The agent selects the output action at time k and interacts with the filtering environment to obtain the position x of the USV at time k+1 s，k+1 , and then through the reward function r k The action is evaluated, and the SAC algorithm learns historical experience through iteration, and finally obtains the optimal trajectory control strategy for USV path planning.

[0244] After executing the optimal action, the USV moves to the corresponding position, obtains the measurement information of the position, and updates the motion status of multiple targets.

[0245] The embodiments of the present disclosure are 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 the embodiments of the present disclosure should be included in the scope of protection of the present disclosure.

Claims

1. An optimal trajectory planning method for an unmanned surface vehicle in a complex environment, characterized in that the steps include: Initialize the state of the target and obtain the initial state information; Based on the initial state information, construct a system model; Construct a threat-based knowledge graph and use the policy gradient reinforcement learning algorithm to infer the threat level and obtain threat level information; Combining the multi-objective posterior Cramer-Rao lower bound and threat information, an evaluation index is constructed; The SAC-based deep reinforcement learning algorithm makes the optimal decision for USV path planning and completes the optimal trajectory planning.

2. The optimal trajectory planning method for unmanned surface vehicles in complex environments according to claim 1 is characterized in that: The method for initializing the state includes: setting the multi-target state set of t targets at k time points to The sampling period is T = 1s, and the process noise matrix v k , assuming that m measurements are received at time k, the multi-target measurement set Z k for The measurement noise matrix w k , the sampling points are N=100.

3. The optimal trajectory planning method for unmanned surface vehicles in complex environments according to claim 2 is characterized in that: The system model includes: a state transfer equation of the target, a measurement equation of the target and an ocean current model; wherein the state transfer equation of the target includes: Where, f(·) represents the state evolution mapping; v k represents process noise; The measurement equation of the target includes: Among them, 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 horizontal coordinate of USV at time k; y s,k represents the ordinate of USV at time k; The ocean current model includes: Among them, u c represents the speed of the ocean current in the y direction; v c represents the speed 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 (ΔT A )=[x o ,y o ] T represents the vortex center vector; ζ represents the vortex intensity; l represents the vortex radius.

4. The optimal trajectory planning method for unmanned surface vehicles in complex environments according to claim 3 is characterized in that: The method of using the policy gradient reinforcement learning algorithm to reason about the threat level includes: constructing a threat assessment dataset, then training the supervision policy network by exploring from a given subset of positive samples, and then, based on the supervision strategy, enabling the agent to learn actions that maximize the reward function to further explore the path of the threat entity from the threat assessment dataset, thereby realizing the reasoning process of the threat level.

5. The optimal trajectory planning method for unmanned surface vehicles in complex environments according to claim 4 is characterized in that: The method for constructing the evaluation index includes: calculating the Cramer-Rao lower bound of all targets based on state priors and covariance matrices to evaluate the performance of multi-target tracking; then inferring the threat degree in real time and calculating the distance between the USV and the target, determining the threat level of each target and ocean current, thereby avoiding target threats and ocean current disturbances, and giving priority to tracking targets with high threats.

6. The optimal trajectory planning method for unmanned surface vehicles in complex environments according to claim 5 is characterized in that: The method for completing the optimal trajectory planning includes: using the SAC intelligent agent to continuously interact with the multi-target tracking filtering environment to obtain the control data of the unmanned surface vehicle to train the intelligent agent, and finally obtaining the action sequence that maximizes the reward function through the trained SAC intelligent agent to decide the optimal trajectory of the unmanned surface vehicle.

7. An optimal trajectory planning system for unmanned surface vehicles in complex environments, the system being used to implement the method described in any one of claims 1 to 6, characterized in that: include: Initialization module, construction module, reasoning module, evaluation module and planning module; The initialization module is used to initialize the state of the target and obtain initial state information; The construction module is used to construct a system model based on the initial state information; The reasoning module is used to construct a threat-based knowledge graph based on the system model, and use a policy gradient reinforcement learning algorithm to reason about the threat level to obtain a reasoning result; The evaluation module is used to construct an evaluation index based on the reasoning result; The planning module is used to complete the optimal trajectory planning based on the evaluation index.

Images