An unmanned ship autonomous return method, device and system based on historical trajectory

Abstract

The application discloses an unmanned ship autonomous return method, device and system based on historical trajectory. t t The method comprises the following steps: acquiring a position point (x t , y t ) of the unmanned ship in real time, removing position points which are approximately overlapped in space at adjacent time points, obtaining an original position point set, searching the original position point set in time sequence, taking position points meeting constraint conditions as connected nodes, and numbering the connected nodes; constructing a topological structure diagram of historical motion trajectory based on the connected nodes, and sequentially calculating distances between the connected nodes; constructing a two-dimensional adjacency matrix according to the topological structure diagram, and searching and determining a shortest path of the historical motion trajectory from a starting point to an ending point and target nodes passed through by using a graph search method; and the unmanned ship autonomously returns according to an autonomous return path planned according to the shortest path and the target nodes. The method is simple, accurate, low in energy consumption, suitable for various complex water area conditions, and capable of improving stability of the unmanned ship when performing a task.

Description

Technical Field

[0001] This invention belongs to the field of unmanned surface vessel (USV) trajectory planning, and more specifically, relates to a method, device, and system for autonomous return of USVs based on historical trajectories. Background Technology

[0002] In recent decades, unmanned surface vessels (USVs) have been widely used in fields such as water quality monitoring, military reconnaissance, and marine exploration due to their high efficiency, high flexibility, and low operating costs.

[0003] In relevant maritime missions, unmanned surface vessels (USVs) have high operational loads and need to complete long-range, wide-area navigation tasks. This means that USVs consume a lot of energy. To save energy, USVs usually use automatic return functions after completing missions. USVs can return along the same route based on their historical navigation trajectory, or they can use onboard cameras and sensors to obtain real-time information about the surrounding environment and perform real-time route planning.

[0004] When faced with insufficient energy or needing to automatically return to port after navigating complex waters, the return-to-port mission of unmanned surface vessels (USVs) becomes extremely challenging. Under conditions of limited energy and sensor system failure, the USV must select the shortest path from its historical trajectory for automatic return to avoid dangerous situations such as engine failure, running aground, or collisions with reefs. Currently, the path planning problem for automatic return of USVs in these situations remains unsolved. Summary of the Invention

[0005] In view of the shortcomings of related technologies, the purpose of this invention is to provide a method, device and system for autonomous return of unmanned surface vessels based on historical trajectories, which aims to solve the problem of how to quickly determine the shortest path for automatic return when unmanned surface vessels have insufficient power or have entered complex waters.

[0006] To achieve the above objectives, in a first aspect, the present invention provides a method for autonomous return of an unmanned surface vessel based on historical trajectories, comprising:

[0007] Real-time acquisition of the unmanned surface vessel's location (x t ,y t By removing spatially overlapping points at adjacent time points, the original set of location points is obtained.

[0008] The original set of location points is searched in chronological order, and the location points that meet the constraints are identified as connected nodes, which are then numbered.

[0009] A topological structure graph of historical motion trajectories is constructed based on the connected nodes, and the distance between each connected node is calculated sequentially.

[0010] A two-dimensional adjacency matrix is ​​constructed based on the topology diagram, and a graph search method is used to search and determine the shortest path and target nodes of the historical movement trajectory from the starting point to the ending point.

[0011] The unmanned surface vessel autonomously returns to its starting point based on the shortest path and the autonomous return path planned by the target node.

[0012] Optionally, the real-time acquisition of the unmanned surface vessel's location (x) t ,y t By removing spatially overlapping points at adjacent time points, the original set of location points is obtained. include:

[0013] By eliminating spatially overlapping points at adjacent time points using a first constraint condition, the following condition is applied: two adjacent point (x...)... t ,y t ) and (x t-1 ,y t-1 The distance between them satisfies:

[0014]

[0015] in, It contains spatial information about historical trajectories, where ε is the threshold for the constraint that temporally adjacent spaces do not overlap;

[0016] The position point (x) that satisfies the first constraint condition t ,y t ) Filtering to temporally adjacent and spatially non-overlapping sequences

[0017] Optionally, the step of searching the original set of location points in chronological order, identifying location points that satisfy the constraints as connected nodes, and numbering the connected nodes includes:

[0018] Entering base point mode, starting from the origin, each point is sequentially set as a base point. The original set of position points is searched forward along the time axis to determine the base point t. now Time and search point time t search To determine if there is spatial overlap, a set of non-overlapping location points is selected. If the time interval between two location points is greater than the minimum time interval for the unmanned surface vessel's return, then they do not overlap. The minimum time interval is:

[0019] Δt circle =πR 2 / V max

[0020] Where R is the minimum turning radius constrained by the unmanned surface vessel's dynamics model, and V max This represents the maximum linear velocity of the unmanned surface vessel.

[0021] Connected nodes are selected from the filtered set of location points using the second constraint; if the base point t now Time and search point time t search If the distance between the corresponding location points satisfies the second constraint condition, then the base point t now Time and search point time t search The corresponding location points are taken as connected nodes, and the second constraint condition is:

[0022]

[0023] The connected nodes obtained from the screening are numbered according to the time sequence.

[0024] Optionally, when filtering connected nodes from the filtered set of location points using the second constraint, the following steps are included:

[0025] The shortest path search algorithm is used to determine the search point at time t from the filtered set of location points. search When the corresponding position point is reached, skip a preset number of position points;

[0026] If the base point t now Time and search point time t search If the distance between the corresponding position points satisfies the third constraint condition, then the search point is far from the reference point, and the next search point skips the first preset number of position points; if the third constraint condition is not satisfied, then the search point skips the second preset number of position points.

[0027] The third constraint is:

[0028] When the third constraint condition is satisfied, the next search time t search ′=t search +n″*T, where n″ represents the distance from the current base point t. now The first preset number of items to skip during a search. t regress The minimum regression safety radius time interval, where T is a time constant;

[0029] When the third constraint is not satisfied, the search time t search =t now +n′*T, where n′ represents the sum of the values ​​of the current base point t. now The preset number of items to skip during a search. Δt circle The minimum time interval for the unmanned surface vessel to regress to this point.

[0030] Optionally, the step of constructing a topology graph of historical motion trajectories based on the connected nodes and sequentially calculating the distances between each connected node includes:

[0031] The connected nodes are used as nodes in a graph structure to construct a topological graph of the historical motion trajectory;

[0032] Each of the connected nodes is decomposed into four connected paths. The time points at which the four paths overlap with the edges of the connected nodes are:

[0033] Constraint matching is performed sequentially for each of the four time points corresponding to the connected nodes to determine the connectivity relationship between the connected nodes and to calculate the distance between the connected nodes.

[0034] Optionally, the step of sequentially performing constraint matching on the four time points corresponding to each connected node to determine the connectivity relationship between connected nodes and calculating the distance between connected nodes includes:

[0035] Let t be the shortest path search algorithm. IN For the connected node numbered n The set, t OUT For the connected node numbered n The set of nodes has the following constraints on the connected nodes numbered n:

[0036]

[0037]

[0038]

[0039]

[0040] in, yes one of the; yes One of them, 0≤n≤N-1;

[0041] The distance between adjacent connected nodes is calculated using the shortest path search algorithm described above:

[0042]

[0043]

[0044]

[0045]

[0046] Where Length(t1,t2) represents the corresponding distance between timest1 andt2.

[0047] Optionally, the step of constructing a two-dimensional adjacency matrix based on the topology graph and using a graph search method to search and determine the shortest path and target nodes traversed by the historical movement trajectory from the starting point to the ending point includes:

[0048] A graph search is performed on the constructed topology graph to determine the shortest path from the starting point to the ending point of the historical motion trajectory;

[0049] The graph search involves constructing a two-dimensional adjacency matrix for the connected nodes in the topological graph using a shortest path search algorithm. The two-dimensional matrix Array[N][N] stores the shortest path lengths between nodes, and Array[N][k] and Array[k][N] (k≠N) take the following values: Where L is the path length between connected nodes.

[0050] Secondly, the present invention also provides an unmanned surface vessel autonomous return device based on historical trajectories, comprising:

[0051] The location set determination module is used to obtain the location (x) of the unmanned surface vessel in real time. t ,y t By removing spatially overlapping points at adjacent time points, the original set of location points is obtained.

[0052] The connected node determination module is used to search the original set of location points in chronological order, identify the location points that meet the constraints as connected nodes, and number the connected nodes.

[0053] The topology graph construction module is used to construct a topology graph of the historical motion trajectory based on the connected nodes, and to calculate the distance between each connected node in sequence.

[0054] The shortest path determination module is used to construct a two-dimensional adjacency matrix based on the topology graph, and to use a graph search method to search and determine the shortest path and target nodes traversed by the historical movement trajectory from the starting point to the ending point.

[0055] The autonomous return module is used by the unmanned surface vessel to autonomously return to its destination based on the autonomous return path planned by the shortest path and the target node.

[0056] Thirdly, the present invention also provides an unmanned surface vessel autonomous return system based on historical trajectories, comprising: a computer-readable storage medium and a processor;

[0057] The computer-readable storage medium is used to store executable instructions;

[0058] The processor is configured to read executable instructions stored in the computer-readable storage medium and execute the method as described in any of the first aspects.

[0059] Fourthly, the present invention also provides a computer-readable storage medium storing computer instructions for causing a processor to perform the method as described in any of the first aspects.

[0060] Compared with the prior art, the above-described technical solutions conceived in this invention can achieve the following beneficial effects:

[0061] 1. This invention provides an autonomous return method for unmanned surface vessels (USVs) based on historical trajectories. The method filters location information by removing overlapping points to eliminate spatial overlap in the time series, resulting in complete, non-overlapping trajectory sequence data. It then searches for location points, using those meeting constraints as connected nodes to construct a topological graph of the historical trajectory. The connections between nodes are converted into a graph form, and an adjacency matrix is ​​established after calculating the distances between nodes. Finally, the shortest path from the starting point to the destination is determined through graph search, and the node sequence is converted into target waypoints to guide the USV's automatic return. This method achieves a simple and accurate trajectory planning approach with low energy consumption, is applicable to various complex water conditions, and improves the stability of USVs during mission execution.

[0062] 2. The present invention provides an autonomous return method for unmanned surface vessels based on historical trajectories. When searching for location points and determining connected nodes, the method judges the selected location points according to two constraints. In different situations, location points can be skipped, thereby reducing the time for selecting connected nodes and improving the efficiency of autonomous return route planning.

[0063] 3. This invention provides an autonomous return method for unmanned surface vessels based on historical trajectories. It constructs a topological graph of historical motion trajectories using connected nodes as nodes in a graph structure. Each connected node's trajectory is decomposed into four connected paths, and the time of the four points corresponding to these four paths is as follows: By utilizing the constraints between adjacent connected nodes at four time points and the shortest path search algorithm, the distance between adjacent connected nodes is calculated; this provides a method for accurately determining the order of connected nodes and calculating the distance between adjacent connected nodes. Attached Figure Description

[0064] Figure 1 This is a flowchart illustrating an autonomous return method for unmanned surface vessels based on historical trajectories provided in an embodiment of the present invention.

[0065] Figure 2 This is a schematic diagram of the structure of an unmanned surface vessel provided in an embodiment of the present invention;

[0066] Figure 3 This is a schematic diagram of the historical motion trajectory in an unmanned surface vessel autonomous return method based on historical trajectory provided in an embodiment of the present invention;

[0067] Figure 4 This is a topological diagram of the historical motion trajectory in an unmanned surface vessel autonomous return method based on historical trajectory provided in an embodiment of the present invention;

[0068] Figure 5 This is a schematic diagram of key nodes in the unmanned surface vessel trajectory map provided in an embodiment of the present invention. Detailed Implementation

[0069] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention. Furthermore, the technical features involved in the various embodiments of this invention described below can be combined with each other as long as they do not conflict with each other.

[0070] The following description, in conjunction with a preferred embodiment, illustrates the content involved in the above embodiments.

[0071] In one embodiment, when determining whether the unmanned surface vessel (USV) is experiencing low battery, has entered complex waters (complex surface environment, severe weather), or needs to perform an automatic return function, the USV shortest path return device based on historical trajectory of this invention is executed, such as... Figure 2 As shown, the device includes: a sensing module (S) capable of acquiring real-time information such as the unmanned surface vessel's (USV) status and position; an upper-level processing unit (U) for storing and computing information about the USV; a shortest path algorithm (P) that can be implemented in the upper-level processing unit; and a lower-level controller (E) for the USV, including waypoint control modes. The device should include external sensors S: various sensing modules capable of acquiring real-time USV position information for control systems; and an upper-level processing unit U: a core processor with a certain computing power. The lower-level controller E should satisfy the following conditions for (x1, y1), (x2, y2), (x... … ,y … ), (x n ,y n This type of spatial sequence of positions allows unmanned surface vessels to be controlled to navigate accurately along a route, based on the dynamics model of the unmanned surface vessel.

[0072] Shortest path algorithm P such as Figure 1 As shown, a method for autonomous return of an unmanned surface vessel based on historical trajectories includes:

[0073] S1. Real-time acquisition of the unmanned surface vessel's location (x)t ,y t By removing spatially overlapping points at adjacent time points, the original set of location points is obtained.

[0074] S2. Search the original set of location points in chronological order, identify the location points that meet the constraints as connected nodes, and number the connected nodes.

[0075] S3. Construct a topology diagram of the historical motion trajectory based on the connected nodes, and calculate the distance between each connected node in sequence;

[0076] S4. Construct a two-dimensional adjacency matrix based on the topology diagram, and use a graph search method to search and determine the shortest path and target nodes of the historical movement trajectory from the starting point to the ending point.

[0077] S5. The unmanned surface vessel autonomously returns to its starting point based on the shortest path and the autonomous return path planned by the target node.

[0078] The unmanned surface vessel (USV) stores its position information from the start of navigation and acquires its position and status information in real time (x). t ,y t It also removes points that are spatially approximately overlapping at adjacent time points in real time, and saves the filtered points to the original location point set. In this embodiment, the GPS data acquisition period of the unmanned surface vessel is T, where T = 0.1s, (x t ,y t ) represents the latitude and longitude (or Cartesian coordinates converted from latitude and longitude) at time t, where t satisfies the constraint: t = n * T, where n is an integer; by constraining the non-overlapping of the continuous spatial neighborhood, positions that are approximately overlapping in space at adjacent times are eliminated.

[0079] When automatically planning the return trajectory, the unmanned surface vessel's calculation modes include a base point mode and a node mode. The base point mode is used to confirm the existence and number of nodes before entering the node mode. After the base point mode ends, the node mode is entered, which is used to determine the relationships between nodes.

[0080] The original set of location points obtained Spatial information containing historical trajectories is searched sequentially over time. Specifically: In base-point mode, starting from the origin, each position point is sequentially set as a base point, and other position points are searched forward along the time axis, with connected nodes identified based on constraints; in node mode, a topological graph of the historical motion trajectory is constructed based on connected nodes, and the distances between connected nodes are calculated. Based on the connected nodes and the distances between nodes, an adjacency matrix is ​​established to represent the graph connections. A graph node search method is applied to the adjacency matrix. First, all nodes are traversed to find the shortest path from the starting point to the ending point. The path sequence is then output sequentially to the lower-level controller of the unmanned surface vessel (USV) containing waypoint control mode. Finally, based on the trajectory tracking algorithm and considering the USV's dynamics model, the USV is guided to accurately navigate along the route.

[0081] Among them, the trajectory tracking algorithm can employ predictive control algorithm, LOS algorithm, and deep learning control algorithm.

[0082] Optionally, step S1, which involves eliminating spatially approximately overlapping location points at adjacent time points, specifically includes:

[0083] By eliminating spatially overlapping points at adjacent time points using a first constraint condition, the following condition is applied: two adjacent point (x...)... t y t ) and (x t-1 y t-1 The distance between them satisfies:

[0084]

[0085] in, It contains spatial information about historical trajectories, where ε is the threshold for the constraint that temporally adjacent spaces do not overlap;

[0086] The position point (x) that satisfies the first constraint condition t y t ) Filtering to temporally adjacent and spatially non-overlapping sequences

[0087] Points that approximately overlap in space at adjacent time points represent coordinates that are continuous on the time axis and whose distance in the spatial domain is less than ε, where ε represents the radius of the constraint region for determining spatial overlap. The coordinates at time t are represented as (x... t ,y t The coordinates at time (t-1) are represented as (x... t-1 ,y t-1 Therefore, the constraint that the neighborhood time-continuous space does not overlap is expressed as:

[0088]

[0089] In one embodiment, ε is set to 1m, and the set is... This represents any set t within the time interval from the start of the flight to the activation of the return-to-home function, i.e., the time when the method ends and saves GPS information. All elements within the range satisfy the aforementioned neighborhood time-continuous spatial non-overlapping constraint.

[0090] Optionally, the step of searching the original set of location points in chronological order, identifying location points that satisfy the constraints as connected nodes, and numbering the connected nodes includes:

[0091] Entering base point mode, starting from the origin, each point is sequentially set as a base point. The original set of position points is searched forward along the time axis to determine the base point t. now Time and search point time t search To determine if there is spatial overlap, a set of non-overlapping location points is selected. If the time interval between two location points is greater than the minimum time interval for the unmanned surface vessel's return, then they do not overlap. The minimum time interval is:

[0092] Δt circle =πR 2 / V max

[0093] Where R is the minimum turning radius constrained by the unmanned surface vessel's dynamics model, and V max This represents the maximum linear velocity of the unmanned surface vessel.

[0094] Connected nodes are selected from the filtered set of location points using the second constraint; if the base point t now Time and search point time t search If the distance between the corresponding location points satisfies the second constraint condition, then the base point t now Time and search point time t search The corresponding location points are taken as connected nodes, and the second constraint condition is:

[0095]

[0096] The connected nodes obtained from the screening are numbered according to the time sequence.

[0097] Optionally, when filtering connected nodes from the filtered set of location points using the second constraint, the following steps are included:

[0098] The shortest path search algorithm is used to determine the search point at time t from the filtered set of location points. search When the corresponding position point is reached, skip a preset number of position points;

[0099] If the base point t now Time and search point time t searchIf the distance between the corresponding position points satisfies the third constraint condition, then the search point is far from the reference point, and the next search point skips the first preset number of position points; if the third constraint condition is not satisfied, then the search point skips the second preset number of position points.

[0100] The third constraint is:

[0101] When the third constraint condition is satisfied, the next search time t search ′=t search +n″*T, where n″ represents the distance from the current base point t. now The first preset number of items to skip during a search. t regress The minimum regression safety radius time interval, where T is a time constant;

[0102] When the third constraint is not satisfied, the search time t search =t now +n′*T, where n′ represents the sum of the values ​​of the current base point t. now The preset number of items to skip during a search. Δt circle The minimum time interval for the unmanned surface vessel to regress to this point.

[0103] In base point mode:

[0104] Starting from the origin and setting it as the base point, search forward along the time axis, with the base point being t. now At time t, set search point t search Continuously judge the base point t now Time and search point time t search Does the space overlap? Consider the unmanned surface vessel (USV) dynamics model with constraints of a minimum turning radius of R and a maximum linear velocity of V. max In one embodiment, R = 3m, V max =10m / s, then the minimum time interval for the unmanned surface vessel to regress to that point is:

[0105] Δt circle =πR 2 / V max (1)

[0106] Therefore, for the current base point t now The number of moments to skip is:

[0107]

[0108] At this point, search time t search =t now Start the search with +n′*T, and determine whether the following constraints are satisfied at each search time step:

[0109]

[0110]

[0111] Wherein, ε′ is the spatial safety radius, and ε″ is the radius of the constraint region for determining spatial overlap conditions during the search phase. In one embodiment, ε′ = 30m and ε″ = 1.5m are set.

[0112] If the above constraint (3) is satisfied, it is considered that the search point is far away from the reference point. Considering the dynamic model of the unmanned surface vessel and its physical constraints, the minimum regression safety radius time interval is:

[0113] Δt regress =ε′ / V max (5)

[0114] For the current search time t serach The number of moments to be skipped is:

[0115]

[0116] Therefore, the next search time is:

[0117] t search =t earch +n″*T (7)

[0118] If constraint (4) is satisfied, then a connected node is considered to have been found, and the current t is recorded. now t start ,

[0119] t in1 =t now (9)

[0120] t in2 =t search (10)

[0121] t out1 =t in1 +T (11)

[0122] t out2 =t in2 +T (12)

[0123] Save records of each connected node and (9), (10), (11), (12);

[0124] If no search point satisfies constraint (4) until the end of the time series, then the next base point is set as:

[0125] tnow =t now +T (8)

[0126] If condition (4) is not met, repeat (1), (2), and (3) until the time series ends and enters node mode.

[0127] Optionally, the step of constructing a topology graph of historical motion trajectories based on the connected nodes and sequentially calculating the distances between each connected node includes:

[0128] The connected nodes are used as nodes in a graph structure to construct a topological graph of the historical motion trajectory;

[0129] Each of the connected nodes is decomposed into four connected paths. The time points at which the four paths overlap with the edges of the connected nodes are:

[0130] Constraint matching is performed sequentially for each of the four time points corresponding to the connected nodes to determine the connectivity relationship between the connected nodes and to calculate the distance between the connected nodes.

[0131] Optionally, the step of sequentially performing constraint matching on the four time points corresponding to each connected node to determine the connectivity relationship between connected nodes and calculating the distance between connected nodes includes:

[0132] Let t be the shortest path search algorithm. IN For the connected node numbered n The set, t OUT For the connected node numbered n The set of nodes has the following constraints on the connected nodes numbered n:

[0133]

[0134]

[0135]

[0136]

[0137] in, yes one of the; yes One of them, 0≤n≤N-1;

[0138] The distance between adjacent connected nodes is calculated using the shortest path search algorithm described above:

[0139]

[0140]

[0141]

[0142]

[0143] Where Length(t1,t2) represents the corresponding distance between timest1 andt2.

[0144] In node mode:

[0145] The number of nodes determined by the base point pattern is N, and each node has four connected paths, such as... Figure 3 As shown, this is the initial trajectory from the starting point to the ending point, with the edges of the solid circles representing the overlapping points within the node space. Constraint matching is performed on the attributes of each node at each time step to find the connectivity relationships and distances between nodes, thereby abstracting a topological graph containing the connectivity relationships between nodes, as shown below. Figure 4 As shown.

[0146] like Figure 5 As shown, the times of the four points where the four paths overlap with the edge of the solid circle are respectively, which are denoted as the four time attributes of node n (0≤n≤N-1): Considering the directionality of time series in the base-point pattern, for each node, there must exist the closest time to its adjacent node in every direction. Let t IN For node numbered n (0≤n≤N-1) The set, t OUT For all The set of connected nodes is given by the constraints on the connected node numbered n as described above. The distance between adjacent connected nodes is calculated using the shortest path search algorithm. From the distance formula above, we can obtain that nodes numbered n (0≤n≤N-1) and k (0≤k≤N-1) are adjacent, and thus obtain the path length between node n (0≤n≤N-1) and node k (0≤k≤N-1).

[0147] Optionally, the step of constructing a two-dimensional adjacency matrix based on the topology graph and using a graph search method to search and determine the shortest path and target nodes traversed by the historical movement trajectory from the starting point to the ending point includes:

[0148] A graph search is performed on the constructed topology graph to determine the shortest path from the starting point to the ending point of the historical motion trajectory;

[0149] The graph search involves constructing a two-dimensional adjacency matrix for the connected nodes in the topological graph using a shortest path search algorithm. The two-dimensional matrix Array[N][N] stores the shortest path lengths between nodes, and Array[N][k] and Array[k][N] (k≠N) take the following values: Where L is the path length between connected nodes.

[0150] For n (0≤n≤N-1) nodes, there are k1, k2, k3, k4 (0≤k 1，2，3，4 There are four branches (≤N-1), and the corresponding path length is set as... An adjacency matrix is ​​established to represent the data structure of the graph connections. Since there are N nodes, a two-dimensional matrix Array[N][N] is set up to store the shortest path lengths between nodes. For nodes with no path, the corresponding adjacency matrix is ​​assigned the value Infinity. In one embodiment, Infinity = 99999m.

[0151] The specific implementation process of using Dijkstra's method to search for nodes is as follows:

[0152] Create an array Prenodes[i] of length N, where Prenodes[i] represents the index of the predecessor node of node i, which minimizes the distance from node i to the starting point; create an array Distance[N] of length N, where Distance[i] represents the shortest distance from node i to the starting point, and initializes it to -1; create an array NotFound[N] of length N, which stores the distances from the starting point to nodes for which no predecessor node has been found. These distances are not necessarily the shortest distances, and initializes the array to 0.

[0153] First, traverse all nodes and find all nodes connected to the starting point. Take the starting point (numbered starIndex) as the predecessor node of these nodes. If node i is connected to the starting point, then:

[0154] Prenodes[i] = startIndex

[0155] NotFound[i]=Array[startIndex][i]

[0156] Then repeat the following steps until the endpoint is found.

[0157] Iterate through the NotFound array and find the smallest element within it:

[0158] NotFound[minIndex] = min

[0159] Set NotFound[minIndex] to Infinity, then update the Distance array to make

[0160] Distance[minIndex] = min

[0161] If the node minIndex is the end point, the loop ends; otherwise, traverse all nodes again. If the shortest distance from the starting point to node i has not been found, and it satisfies:

[0162] Distance[minIndex]+Array[minIndex][i]<NotFound[i] or

[0163] NotFound[i]=Infinity

[0164] Then:

[0165] NotFound[i]=Distance[minIndex]+Array[minIndex][i]

[0166] And take the node minIndex as the predecessor node of node i, that is:

[0167] Prenodes[i]=minIndex

[0168] After the above loop ends, return the array Prenodes. By indexing layer by layer from the end point, the shortest path from the starting point to the end point can be found.

[0169] In the embodiment of the present invention, by screening the position information, removing the overlapping position points, removing the spatial overlapping part in the time series, and obtaining the complete and non-overlapping trajectory sequence data; searching for the position points, taking the position points that meet the constraint conditions as connected nodes, thereby constructing the topological structure diagram of the historical movement trajectory, converting the connection between nodes into the form of a graph, calculating the distance between nodes, and then establishing an adjacency matrix; finally, determining the shortest path from the starting point to the end point through graph search, and converting the node order into the target waypoint to guide the unmanned aerial vehicle to return automatically, which solves the problem of how to quickly determine the shortest path for automatic return when the unmanned boat has insufficient energy and power and strays into complex waters, realizes the advantages of simple and accurate trajectory planning method, low energy consumption, applicable to various complex water conditions, and improves the stability of the unmanned boat when performing tasks.

[0170] Based on the above embodiments, the present invention also provides an unmanned boat autonomous return device based on historical trajectories, including:

[0171] Position point set determination module, used to obtain the position points (x t , y t ) of the unmanned boat in real time, remove the position points with approximately overlapping space at adjacent moments, and obtain the original position point set

[0172] The connected node determination module is used to search the original set of location points in chronological order, identify the location points that meet the constraints as connected nodes, and number the connected nodes.

[0173] The topology graph construction module is used to construct a topology graph of the historical motion trajectory based on the connected nodes, and to calculate the distance between each connected node in sequence.

[0174] The shortest path determination module is used to construct a two-dimensional adjacency matrix based on the topology graph, and to use a graph search method to search and determine the shortest path and target nodes traversed by the historical movement trajectory from the starting point to the ending point.

[0175] The autonomous return module is used by the unmanned surface vessel to autonomously return to its destination based on the autonomous return path planned by the shortest path and the target node.

[0176] The unmanned surface vessel (USV) includes a positioning module, an inertial navigation module, a storage module, a computing module, and a power supply module. The modules in the historical trajectory-based autonomous return-to-home device (position point set determination module, connected node determination module, topology graph construction module, shortest path determination module, and autonomous return-to-home module) are all sub-modules of the computing module. They are used to plan a shortest path based on the USV's historical navigation data and control the USV to autonomously navigate back to the starting point based on the shortest path. The positioning module detects the USV's current latitude and longitude coordinates. The inertial navigation module detects the USV's current attitude information, including roll, pitch, and heading angles. The storage module stores the historical latitude and longitude data of the USV during its navigation. The power supply module provides a stable power supply to all the above modules.

[0177] The storage module continuously stores the unmanned surface vessel's (USV) position and attitude data from its starting point. When the USV needs to perform autonomous return, runs out of energy, or accidentally enters waters with complex environments, the computing module retrieves the USV's historical data from the storage module and uses a shortest path search algorithm to plan the path. Then, it uses a trajectory tracking algorithm to estimate and track the USV, guiding it to return to port autonomously along a given trajectory sequence.

[0178] Based on the above embodiments, the present invention also provides an unmanned surface vessel autonomous return system based on historical trajectory, comprising: a computer-readable storage medium and a processor;

[0179] The computer-readable storage medium is used to store executable instructions;

[0180] The processor is configured to read executable instructions stored in the computer-readable storage medium and execute the method described in any of the above embodiments.

[0181] Based on the above embodiments, the present invention also provides a computer-readable storage medium storing computer instructions for causing a processor to perform the method described in any of the above embodiments.

[0182] Those skilled in the art will readily understand that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for autonomous return of an unmanned surface vessel based on historical trajectories, characterized in that, include: Real-time location of unmanned surface vessels By removing spatially overlapping points at adjacent time points, the original set of points is obtained. ; The original set of location points is searched in chronological order, and the location points that meet the constraints are identified as connected nodes, which are then numbered. A topological structure graph of historical motion trajectories is constructed based on the connected nodes, and the distance between each connected node is calculated sequentially. A two-dimensional adjacency matrix is ​​constructed based on the topology diagram, and a graph search method is used to search and determine the shortest path and target nodes of the historical movement trajectory from the starting point to the ending point. The unmanned surface vessel autonomously returns to its starting point based on the shortest path and the target node's planned autonomous return path. The step of searching the original set of location points in chronological order, identifying location points that satisfy the constraints as connected nodes, and numbering these connected nodes includes: Entering the base point mode, starting from the origin, each point is sequentially set as a base point. The original set of position points is searched forward along the time axis to determine the base point. Time and search point time To determine if there is spatial overlap, a set of non-overlapping location points is selected. If the time interval between two location points is greater than the minimum time interval for the unmanned surface vessel's return, then they do not overlap. The minimum time interval is: in, To constrain the minimum turning radius of the unmanned surface vessel's dynamics model, This represents the maximum linear velocity of the unmanned surface vessel. Connected nodes are selected from the filtered set of location points based on the second constraint; if the base point Time and search point time If the distance between the corresponding location points satisfies the second constraint condition, then the base point Time and search point time The corresponding location points are taken as connected nodes, and the second constraint condition is: ； The connected nodes obtained from the screening are numbered according to their time series. The radius of the constraint region used to determine spatial overlap conditions during the search phase.

2. The method as described in claim 1, characterized in that, The real-time acquisition of the unmanned surface vessel's location By removing spatially overlapping points at adjacent time points, the original set of points is obtained. ,include: The first constraint condition is used to eliminate spatially overlapping points at adjacent time points. This first constraint condition is: two adjacent point... and The distance between them satisfies: in, Spatial information containing historical trajectories. The threshold for temporally adjacent spatial non-overlapping constraints; The position point that satisfies the first constraint condition Filtering to temporally adjacent and spatially non-overlapping sequences .

3. The method as described in claim 1, characterized in that, When filtering connected nodes from the filtered set of location points using the second constraint, the process includes: The shortest path search algorithm is used to determine the search point from the filtered set of location points. When the corresponding position point is reached, skip a preset number of position points; If the base point Time and search point time If the distance between the corresponding position points satisfies the third constraint condition, then the search point is far away from the reference point, and the next search point skips the first preset number of position points; if the third constraint condition is not satisfied, then the search point skips the second preset number of position points. The third constraint is: ; When the third constraint is satisfied, the next search time ,in, For the current base point The first preset number of items to skip during a search. , The minimum regression safety radius time interval, It is a time constant; When the third constraint is not satisfied, the search time is... ,in, For the current base point The preset number of items to skip during a search. , The minimum time interval for the unmanned surface vessel to regress to this point.

4. The method as described in claim 1, characterized in that, The process of constructing a topological graph of historical motion trajectories based on the connected nodes and sequentially calculating the distances between each connected node includes: The connected nodes are used as nodes in a graph structure to construct a topological graph of the historical motion trajectory; Each of the connected nodes is decomposed into four connected paths. The time points at which the four paths overlap with the edges of the connected nodes are: ; Constraint matching is performed sequentially for each of the four time points corresponding to the connected nodes to determine the connectivity relationship between the connected nodes and to calculate the distance between the connected nodes.

5. The method as described in claim 4, characterized in that, The step of sequentially performing constraint matching for each of the four time points corresponding to a connected node to determine the connectivity relationship between connected nodes and calculating the distance between connected nodes includes: Using the shortest path search algorithm For the number Connected nodes The set, For the number Connected nodes The set, for the numbered The constraints for the connected nodes are: in, yes one of the; yes one of the, ; The number of nodes; The distance between adjacent connected nodes is calculated using the shortest path search algorithm described above: in, represent , The corresponding distance between moments.

6. The method as described in claim 1, characterized in that, The step of constructing a two-dimensional adjacency matrix based on the topology graph and using a graph search method to determine the shortest path and target nodes along the historical movement trajectory from the starting point to the ending point includes: A graph search is performed on the constructed topology graph to determine the shortest path from the starting point to the ending point of the historical motion trajectory; The graph search involves constructing a two-dimensional adjacency matrix for the connected nodes in the topological graph using a shortest path search algorithm. The shortest path length between storage nodes , The value can be: , where L is the path length between connected nodes.

7. An autonomous return-to-home device for unmanned surface vessels based on historical trajectories, characterized in that, include: The location set determination module is used to obtain the location of the unmanned surface vessel in real time. By removing spatially overlapping points at adjacent time points, the original set of points is obtained. ; The connected node determination module is used to search the original set of location points in chronological order, identify the location points that meet the constraints as connected nodes, and number the connected nodes. The topology graph construction module is used to construct a topology graph of the historical motion trajectory based on the connected nodes, and to calculate the distance between each connected node in sequence. The shortest path determination module is used to construct a two-dimensional adjacency matrix based on the topology graph, and to use a graph search method to search and determine the shortest path and target nodes traversed by the historical movement trajectory from the starting point to the ending point. An autonomous return module is used for the unmanned surface vessel to autonomously return to its destination based on the shortest path and the target node's planned autonomous return path. The step of searching the original set of location points in chronological order, identifying location points that satisfy the constraints as connected nodes, and numbering these connected nodes includes: Entering the base point mode, starting from the origin, each point is sequentially set as a base point. The original set of position points is searched forward along the time axis to determine the base point. Time and search point time To determine if there is spatial overlap, a set of non-overlapping location points is selected. If the time interval between two location points is greater than the minimum time interval for the unmanned surface vessel's return, then they do not overlap. The minimum time interval is: in, To constrain the minimum turning radius of the unmanned surface vessel's dynamics model, This represents the maximum linear velocity of the unmanned surface vessel. Connected nodes are selected from the filtered set of location points based on the second constraint; if the base point Time and search point time If the distance between the corresponding location points satisfies the second constraint condition, then the base point Time and search point time The corresponding location points are taken as connected nodes, and the second constraint condition is: ； The connected nodes obtained from the screening are numbered according to their time series. The radius of the constraint region used to determine spatial overlap conditions during the search phase.

8. An autonomous return system for unmanned surface vessels based on historical trajectories, characterized in that, include: Computer-readable storage media and processors; The computer-readable storage medium is used to store executable instructions; The processor is configured to read executable instructions stored in the computer-readable storage medium and execute the method as described in any one of claims 1-6.

9. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer instructions for causing a processor to perform the method as described in any one of claims 1-6.

Images