A spatiotemporal tensor mission priority local planning method for unmanned surface vehicle

Abstract

The application provides a spatiotemporal tensor task priority local planning method for unmanned surface vehicle, and belongs to the technical field of autonomous unmanned system navigation and control, and comprises the following steps: constructing a unified three-dimensional spatiotemporal tensor based on environment perception data; the three-dimensional spatiotemporal tensor integrally encodes static obstacle information and future prediction occupation information of dynamic obstacles, supports constant time complexity collision feasibility query of any position and time combination; generating guidance reference information containing expected heading based on time parameterized reference trajectory; generating a candidate control action set based on the current motion state, performing collision constraint verification on the candidate control action based on the three-dimensional spatiotemporal tensor, screening to obtain a safe action set satisfying the non-collision constraint, and selecting an optimal control action in the safe action set with the only optimization target of minimizing the deviation between the predicted ship heading and the expected heading; and step four, generating a local planning path based on the optimal control action, and controlling the unmanned surface vehicle to sail.

Description

Technical Field

[0001] This invention belongs to the field of navigation and control technology for autonomous unmanned systems, and specifically relates to a spatiotemporal tensor task-priority local planning method for unmanned surface vessels. Background Technology

[0002] With the continuous increase in maritime traffic density, navigation safety and operational efficiency have become increasingly prominent issues, driving the rapid development of autonomous navigation systems. However, the local marine environment remains highly complex and uncertain. The coexistence of static and dynamic obstacles and environmental disturbances further increase the difficulty of autonomous navigation, and these factors often cause unmanned surface vessels to deviate from their intended routes during operation.

[0003] In collaborative or time-constrained mission scenarios, such as formation sailing, multi-ship collaborative sampling, and arrival missions with time window constraints, the reference motion of a ship is defined not only spatially but also parameterized in the temporal dimension. Therefore, collision avoidance requires not only ensuring geometric safety but also maintaining the ship's mission progress consistency along its time-parameterized trajectory. If a ship deviates from the reference trajectory during collision avoidance and only subsequently returns to the geometric path, time tracking errors accumulate. These deviations can lead to coordination failures, mission delays, or reduced mission effectiveness. Therefore, the collision avoidance process must maintain consistency with the reference trajectory in both spatial and temporal dimensions.

[0004] Existing local collision avoidance methods for unmanned surface vessels (USVs) are typically based on geometric or kinematic path planning. Traditional methods such as velocity obstacle (VO) methods, dynamic window (DWA) methods, and planners based on artificial potential fields (APF) are widely used in practice. However, these methods generally have typical limitations, such as susceptibility to local minima, insufficient adaptability in complex interaction scenarios, and poor trajectory smoothness. To alleviate these problems, recent research has proposed various improvement methods. For example, the potential field model is enhanced by introducing environmental density and spatial risk metrics to suppress oscillations and avoid unwanted local convergence. A learning mechanism is introduced into the DWA framework to achieve online adaptive adjustment of evaluation weights, thereby reducing manual parameter tuning. In addition, sampling planning methods also improve the search strategy and trajectory generation mechanism to accelerate convergence and generate smoother paths. Although the above methods improve planning performance to some extent, they are still essentially planning-driven paradigms, with the collision avoidance process mainly modeled at the geometric or kinematic level. At the same time, planning methods based on deep reinforcement learning (DRL) are gradually emerging, which reduce human intervention and improve policy flexibility through self-learning capabilities. However, these methods typically require large amounts of training data, and their generalization ability and robustness remain limited when facing unknown dynamic environments or changing distributions. To address this, hybrid methods combining DRL with traditional models have emerged in recent years. For example, combining reinforcement learning with Reciprocal Velocity Obstacle (RVO) theory can more effectively handle both static and dynamic obstacles. However, generally speaking, both traditional and learning-based methods treat collision avoidance as an independent planning problem, without explicitly considering closed-loop trajectory tracking and task-level time constraints. Therefore, even if the ship successfully avoids the obstacle and returns to the reference path, the time deviation generated during the collision avoidance process may still prevent it from meeting the requirements of time-sensitive tasks.

[0005] Unlike planning-based approaches, another type of research directly integrates collision avoidance into the control layer. For example, some schemes use VO (Voice of Target) and vector field methods as guidance laws, enabling ships to avoid static obstacles while following a path; others improve the line-of-sight (LOS) algorithm based on geometric relationships to achieve obstacle avoidance. In these guidance law-based methods, collision avoidance is mainly achieved by guiding the ship back to the reference path, but without explicitly constraining its timeline. In recent years, optimization-based control methods (such as nonlinear model predictive control, NMPC) have also been used to unify trajectory tracking and collision avoidance. For example, some schemes directly embed collision avoidance constraints into the NMPC framework, achieving integrated control to a certain extent and exhibiting good tracking performance. However, these methods typically have high computational costs. To reduce computational burden, some schemes introduce event-triggered mechanisms to reduce the frequency of optimization solutions; others enhance obstacle avoidance capabilities in complex environments by introducing adaptive mechanisms and short-term dynamic predictions. Despite these advances, directly coupling collision avoidance to the control layer still has inherent structural limitations. While these methods can impose spatial collision constraints, they do not explicitly consider temporal consistency issues after collision avoidance maneuvers. When a ship slows down or yaws to avoid a collision, it is prone to time lag, which can lead to it being out of sync with the progress of time-sensitive tasks.

[0006] In summary, despite the progress made by existing technologies, controlling the time progression according to the reference trajectory remains a key challenge during obstacle avoidance. Existing methods typically only ensure that the ship returns to the reference path spatially, without constraining its time progress. This results in the ship being spatially realigned but temporally deviating, thus affecting formation coordination, sampling timing, and mission completion. Summary of the Invention

[0007] To address the shortcomings and deficiencies of existing technologies, this invention provides a task-priority local planning method and system for time-parameterized trajectory tracking of unmanned surface vessels (USVs). It primarily solves the problems of existing USV collision avoidance schemes, which struggle to balance spatial collision avoidance safety with the temporal consistency of time-parameterized mission trajectories, suffer from high computational overhead in collision detection under dynamic and complex marine environments, lack real-time performance, and are prone to mission failure due to accumulated time deviations during collision avoidance maneuvers. This invention constructs a unified three-dimensional spatiotemporal tensor to integrate static obstacle information with the predicted future occupancy information of dynamic obstacles, enabling rapid low-complexity collision feasibility queries for any combination of location and time. Simultaneously, it generates an environment representation with hierarchical safety semantics through two morphological dilation operations, providing a reliable safety constraint benchmark for collision avoidance planning. Based on this, a task-priority decision logic is adopted. Within the safe action range that satisfies collision-free constraints, the optimal control action is selected with minimizing the deviation between the predicted ship's course and the desired course for trajectory tracking guidance as the core optimization objective. Furthermore, it combines neighborhood-based candidate action generation, longitudinal velocity adaptive adjustment, and a multi-step path search mechanism with backtracking to minimize the disturbance of collision avoidance maneuvers to the predetermined mission progress while ensuring collision avoidance safety. This invention can be effectively adapted to time-sensitive unmanned surface vessel operation scenarios such as maritime formation navigation, multi-ship collaborative sampling, and navigation missions with time window constraints, and has both good collision avoidance reliability and mission execution stability in complex and dynamic marine environments.

[0008] The specific technical solution adopted by this invention to solve its technical problem is as follows:

[0009] A spatiotemporal tensor task-priority local planning method for unmanned surface vessels includes acquiring the current motion state of the unmanned surface vessel, a preset time-parameterized reference trajectory, and environmental perception data collected by onboard sensors, and further includes the following steps:

[0010] Step 1: Construct a unified three-dimensional spatiotemporal tensor based on environmental perception data; the three-dimensional spatiotemporal tensor encodes static obstacle information and future predicted occupancy information of dynamic obstacles in an integrated manner, supporting collision feasibility queries with constant time complexity for any combination of position and time.

[0011] Step 2: Generate guidance reference information containing the desired heading based on the time-parameterized reference trajectory;

[0012] Step 3: Generate a set of candidate control actions based on the current motion state, perform collision constraint verification on the candidate control actions based on the three-dimensional spatiotemporal tensor, and filter to obtain a set of safe actions that meet the collision-free constraints. In the set of safe actions, the optimal control action is selected with minimizing the deviation between the predicted ship's course and the desired course as the sole optimization objective.

[0013] Step 4: Generate a local planning path based on the optimal control action to control the unmanned surface vessel's navigation.

[0014] Furthermore, the three-dimensional spatiotemporal tensor is a multi-layered three-dimensional structure formed by expanding a spatial grid centered on the unmanned surface vessel along the time dimension. The preceding layers store dynamic obstacle occupancy status information in the current time and future prediction time domain, while the last layer stores static obstacle information.

[0015] Furthermore, when constructing a unified three-dimensional spatiotemporal tensor, two morphological dilation operations are performed on the initial occupied tensor to generate a physically safe boundary and a robustly safe boundary, respectively. Finally, the three-dimensional spatiotemporal tensor is assigned three-level semantic values, where the value 0 corresponds to the free region, the value 1 corresponds to the physically safe boundary, and the value 2 corresponds to the robustly safe boundary.

[0016] Furthermore, the candidate control action is generated as follows: taking the current bow angular velocity of the unmanned surface vessel as a reference, in the preset discrete bow angular velocity action space, the reference action that is closest to the current bow angular velocity, as well as the previous adjacent action and the next adjacent action of the reference action, are selected to form a set of candidate control actions.

[0017] Furthermore, the collision constraint verification rule is as follows: the candidate control action is determined to satisfy the no-collision constraint only when the spatiotemporal tensor values ​​of the layer storing dynamic obstacle occupancy state information and the layer storing static obstacle information at the corresponding prediction time of the predicted ship position corresponding to the candidate control action are all 0.

[0018] Furthermore, the evaluation function corresponding to the optimization objective is:

[0019]

[0020] in, Candidate actions The evaluation value, The predicted heading angle of the ship corresponding to the candidate maneuvers. The desired heading angle is given in the guidance reference information; the candidate action with the smallest evaluation value is selected as the optimal control action.

[0021] Furthermore, the single-step decision-making process in step three is embedded in a depth-first multi-step search framework with backtracking: the state tree is gradually expanded within the preset planning time domain to generate local planning paths. When a branch has no candidate action that satisfies the collision-free constraint, it backtracks to the previous state to explore other candidate actions until a complete feasible local planning path is generated or it backtracks to the initial state.

[0022] Furthermore, when performing forward prediction based on candidate control actions, longitudinal speed adjustment is performed simultaneously: when the desired speed in the guidance reference information is greater than the current speed, acceleration compensation is limited to the ship's maximum acceleration; when the desired speed is less than or equal to the current speed, deceleration adjustment is limited to the ship's maximum deceleration to compensate for the time delay generated during collision avoidance.

[0023] And, a spatiotemporal tensor task-priority local programming system for unmanned surface vessels, comprising:

[0024] The data acquisition module is used to acquire the current motion state of the unmanned surface vessel, the preset time parameterized reference trajectory, and the environmental perception data collected by the shipborne sensors.

[0025] The spatiotemporal tensor construction module is used to construct a unified three-dimensional spatiotemporal tensor based on environmental perception data. The three-dimensional spatiotemporal tensor encodes static obstacle information and future predicted occupancy information of dynamic obstacles in an integrated manner, supporting collision feasibility queries with constant time complexity for any combination of position and time.

[0026] The guidance reference generation module is used to generate guidance reference information containing the desired heading based on the time-parameterized reference trajectory.

[0027] The candidate action generation module is used to generate a set of candidate control actions based on the current motion state;

[0028] The task priority decision module is used to perform collision constraint verification on candidate control actions based on the three-dimensional spatiotemporal tensor, filter to obtain a set of safe actions that meet the collision-free constraints, and select the optimal control action in the set of safe actions with minimizing the deviation between the predicted ship course and the desired course as the sole optimization objective.

[0029] The navigation control module is used to generate a local planned path based on the optimal control actions to control the navigation of the unmanned surface vessel.

[0030] Furthermore, the spatiotemporal tensor construction module is specifically used to project the lidar point cloud to generate a two-dimensional occupancy grid centered on the unmanned surface vessel, use a constant velocity model to predict the motion of dynamic obstacles, and perform two morphological dilation operations on the initial occupancy tensor to generate a three-dimensional spatiotemporal tensor with three-level safety semantics.

[0031] And a computer system including a processor and a memory, the memory storing a computer program, the processor executing the computer program to implement the method described above.

[0032] A non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method described above.

[0033] Compared to existing technologies, this invention and its preferred solution, at the environmental perception and representation level, achieve integrated modeling of static and dynamic obstacle prediction information through a unified three-dimensional spatiotemporal tensor. This transforms complex collision geometry calculations into low-complexity, fast queries, effectively reducing the computational overhead of collision avoidance planning in dynamic marine environments. Furthermore, by setting hierarchical safety semantics, it provides a constraint benchmark for collision avoidance planning that balances basic safety with environmental uncertainties, improving the robustness of planning in complex environments. At the planning decision logic level, a task-first decision mechanism is adopted, using collision-free safety as a hard constraint and course consistency as the core optimization objective. While achieving reliable collision avoidance, it minimizes the disturbance of collision avoidance maneuvers to the predetermined time parameterized task, effectively solving the problems of existing technologies. The solution addresses the pain points of collision avoidance operations accumulating time deviations and disrupting mission sequence continuity, making it better suited for time-constrained maritime operation scenarios. At the engineering implementation level, through neighborhood-based candidate action generation, a multi-step search framework with backtracking, and adaptive speed adjustment, the solution further reduces the computational burden of planning while ensuring planning effectiveness. This adapts to the computational constraints of the unmanned surface vessel's onboard hardware and aligns with the vessel's kinematic characteristics, enhancing the solution's engineering practicality and the smoothness of navigation control. The overall solution constructs a complete closed loop from environmental modeling and planning decisions to navigation execution, balancing navigation safety and mission execution efficiency. It is adaptable to various typical maritime encounter scenarios and trajectory tracking tasks, effectively improving the stability and mission execution capabilities of the unmanned surface vessel's autonomous navigation system. Attached Figure Description

[0034] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments:

[0035] Figure 1 This is a schematic diagram of the coordinate system of an unmanned surface vessel according to an embodiment of the present invention;

[0036] Figure 2 This is a schematic diagram of the overall architecture of the task-priority local planning framework according to an embodiment of the present invention;

[0037] Figure 3 This is a schematic diagram of the unified spatiotemporal tensor construction process in an embodiment of the present invention; in the figure, (a) is a schematic diagram of the local occupancy grid centered on the unmanned surface vessel, (b) is a schematic diagram of the short-term motion prediction of dynamic obstacles, (c) is a schematic diagram of the hierarchical security semantics of the spatiotemporal tensor, and (d) is a schematic diagram of the structure of the three-dimensional spatiotemporal tensor.

[0038] Figure 4 This is a schematic diagram of the task-priority local planning decision logic in an embodiment of the present invention;

[0039] Figure 5The figure shows the simulation results of collision avoidance performance under the straight reference trajectory scenario in the embodiment of the present invention; in the figure, (a) is a schematic diagram comparing the actual navigation trajectory of the ship with the reference trajectory, (b) is the curve of the change of the bow roll angular velocity during navigation, (c) is the curve of the change of the longitudinal velocity during navigation, (d) is the curve of the change of the track error during navigation, and (e) is the curve of the change of the lateral error during navigation.

[0040] Figure 6 The figure shows the simulation results of collision avoidance performance under the circular reference trajectory scenario in the embodiment of the present invention; in the figure, (a) is a schematic diagram comparing the actual navigation trajectory of the ship with the reference trajectory, (b) is the curve of the change of the bow roll angular velocity during navigation, (c) is the curve of the change of the longitudinal velocity during navigation, (d) is the curve of the change of the lateral error during navigation, and (e) is the curve of the change of the track error during navigation.

[0041] Figure 7 The figure shows the simulation results of collision avoidance performance under the complex S-shaped reference trajectory scenario of the present invention. In the figure, (a) is a schematic diagram comparing the actual navigation trajectory of the ship with the reference trajectory, (b) is the curve of the change of the bow roll angular velocity during navigation, (c) is the curve of the change of the longitudinal velocity during navigation, (d) is the curve of the change of the lateral error during navigation, and (e) is the curve of the change of the track error during navigation.

[0042] Figure 8 This is a physical image of the unmanned surface vessel test platform used in the sea trials of this invention.

[0043] Figure 9 The figure shows the collision avoidance performance test results in a maritime encounter scenario according to an embodiment of the present invention; in the figure, (a) is a schematic diagram comparing the actual navigation trajectory of the ship with the reference trajectory, (b) is the curve of the change of the bow roll angular velocity during navigation, (c) is the curve of the change of the longitudinal velocity during navigation, (d) is the curve of the change of the lateral error during navigation, and (e) is the curve of the change of the track error during navigation.

[0044] Figure 10 The figure shows the test results of collision avoidance performance in a maritime cross-encounter scenario according to an embodiment of the present invention; in the figure, (a) is a schematic diagram comparing the actual navigation trajectory of the ship with the reference trajectory, (b) is the curve of the change of the bow roll angular velocity during navigation, (c) is the curve of the change of the longitudinal velocity during navigation, (d) is the curve of the change of the lateral error during navigation, and (e) is the curve of the change of the track error during navigation.

[0045] Figure 11The figure shows the collision avoidance performance test results in a maritime overtaking and encounter scenario according to an embodiment of the present invention. In the figure, (a) is a schematic diagram comparing the actual navigation trajectory of the ship with the reference trajectory, (b) is the curve of the change of the bow roll angular velocity during navigation, (c) is the curve of the change of the longitudinal velocity during navigation, (d) is the curve of the change of the lateral error during navigation, and (e) is the curve of the change of the track error during navigation. Detailed Implementation

[0046] To make the features and advantages of the present invention more apparent and understandable, specific embodiments are described below in detail:

[0047] It should be noted that the following detailed descriptions are exemplary and intended to provide further explanation of this application. Unless otherwise specified, all technical and scientific terms used in this specification have the same meaning as commonly understood by one of ordinary skill in the art to which this application pertains.

[0048] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the exemplary embodiments according to this application. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0049] This invention addresses the conflict between collision avoidance requirements and mission continuity in time-parameterized trajectory tracking tasks for unmanned surface vessels (USVs). It proposes a local planning framework that combines a unified spatiotemporal tensor with a mission-priority decision-making mechanism. During collision avoidance, it explicitly considers maintaining the progress of the time-parameterized trajectory to ensure both collision safety and mission temporal consistency. This method is based on a unified spatiotemporal environment representation and incorporates a guidance law-driven decision-making strategy to minimize disturbances to the established mission progress while ensuring collision avoidance safety. The framework employs a three-dimensional (x,y,t) spatiotemporal representation method, unifying the short-term predictions of static and dynamic obstacles into a single model. This enables constant-time feasibility queries for any position-time combination. Furthermore, the planner integrates guidance law-based reference information and performs a forward feasibility search on a finite set of candidate control actions, selecting the maneuver scheme that minimizes interference with the established mission progress. This strategy effectively reduces spatial and temporal offsets relative to the reference trajectory during collision avoidance by minimizing the heading deviation relative to the guidance command. Simulation and real-ship experiments show that the method proposed in this invention can not only achieve reliable collision avoidance in complex and dynamic marine environments, but also maintain good time consistency, and has application prospects in time-sensitive maritime missions.

[0050] The main contributions of this proposal are as follows:

[0051] (1) Unified spatiotemporal tensor representation: A unified three-dimensional environment representation method is proposed, which unifies the modeling of static obstacles and dynamic obstacle prediction under the same representation framework, and realizes collision feasibility query for real-time planning.

[0052] (2) Task-first local planning algorithm: A local planning method based on guidance law is proposed. It evaluates the limited set of candidate heading angular velocity commands and selects the maneuver strategy that is most consistent with the guidance direction under the premise of satisfying the collision-free constraint, so as to maintain the mission advancement process along the reference trajectory.

[0053] (3) Integrated framework and experimental verification: Construct a complete integrated method framework and verify its superior balance between safety and mission consistency through a large number of simulation and real ship experiments.

[0054] The specific implementation process of this scheme is further described as follows: 1. Modeling the local path planning problem with task continuity constraints; 2. The proposed unified spatiotemporal tensor environment representation method; 3. The task-priority local planning algorithm is given; 4. The performance of the method is fully verified and analyzed through simulation and real ship experiments.

[0055] 1. Problem Statement and Modeling

[0056] 1.1. Ship Kinematic Model

[0057] The model is based on a three-degree-of-freedom (3-DOF) model, which considers longitudinal velocity in the ship's coordinate system. lateral velocity and bow roll rate And describe the position in the inertial coordinate system. and heading angle The relationship between a ship's speed and its attitude can be expressed as:

[0058] (1)

[0059] in Let represent the pose in the inertial coordinate system. The left side of formula (1) represents the rate of change of the pose with respect to time, i.e., the velocity in the inertial coordinate system. This represents the velocity in the ship's coordinate system. The rotation matrix from the ship's coordinate system to the inertial coordinate system. It is given by the following formula:

[0060] (2)

[0061] The coordinate system used in this scheme is as follows: Figure 1As shown, O0-x0y0 is the geodetic inertial coordinate system, used to describe the position and heading of the unmanned surface vessel in global space, and O-xy is the body coordinate system attached to the hull, used to describe the longitudinal, lateral velocities, and bow angular velocity of the unmanned surface vessel. To achieve real-time local planning, a simplified discrete-time kinematic model is adopted to support forward prediction of multiple candidate actions. In this planning layer model, the control inputs only include the longitudinal velocity *r* and the bow angular velocity *r*. Its discrete state transition relationship is expressed as:

[0062]

[0063] (3)

[0064]

[0065] Where k is the discrete time step number and Δt is the planning time step size.

[0066] This model provides efficient forward prediction capabilities for short-time local programming.

[0067] 1.2. Trajectory Tracking Guidance Law

[0068] Based on existing guidance laws, the track error x e and lateral error y e Defined as:

[0069] (4)

[0070] in, Indicates time Reference trajectory points, Let be the tangential angle of the desired path. Then, the desired heading angle is used for local planning. and desired longitudinal velocity u p Expressed as:

[0071] (5)

[0072] in, >0 indicates adaptive forward look distance. For the sideslip angle, U p For the desired speed U d Intermediate variables obtained from tracking error calculations. These guidance commands. Serves as a reference input for the local planner in the following chapters.

[0073] 1.3. Problem Statement

[0074] Existing local path planning methods for unmanned surface vessels primarily focus on collision avoidance and path optimization, aiming to ensure navigation safety and improve path efficiency. However, these methods pay less attention to maintaining mission continuity after collision avoidance maneuvers. Therefore, frequently triggered collision avoidance behaviors gradually accumulate into significant spatiotemporal deviations, causing the vessel to gradually deviate from the preset mission trajectory and disrupting temporal coordination during mission execution.

[0075] To address the aforementioned issues, this proposal suggests a task-priority local path planning framework that balances collision safety and task consistency. Formally, at each moment, the ship's state is defined as... Given the current state and the time-parameterized reference trajectory. Under the given conditions, the planner solves for a set of bow roll rate commands under the collision-free constraints encoded by the spacetime tensor M (whose formal definition is given in Chapter 2 below), so that the predicted heading aligns with the navigation heading. The deviation between them is minimal.

[0076] like Figure 2 As shown, this framework integrates information from multiple modules. The time-parameterized trajectory tracking module (bottom left) continuously provides the desired motion state. As a planning reference, the perception data from shipboard sensors (top left) is uniformly represented as a spatiotemporal tensor M (middle) to encode short-term obstacle predictions and support rapid risk assessment. The lower middle section illustrates the local replanning process, where the ship reverts to its time-parameterized mission trajectory after avoiding an obstacle. Figure 2 The process sequence on the right further demonstrates that the ship achieves time resynchronization with the reference trajectory during the collision avoidance process through acceleration and deceleration during the return phase. The effectiveness of the proposed framework was verified through simulation and live-ship experiments. Specific implementation details and experimental results are provided in subsequent chapters.

[0077] 2.2. Spacetime Tensor

[0078] To support obstacle avoidance and navigation for unmanned surface vessels (USVs) in dynamic marine environments, this embodiment constructs a unified spatiotemporal tensor M as an environmental representation for real-time local planning. This tensor discretizes the local map centered on the USV into a single... A grid, which expands along the time dimension to form Layered structure. Among them, layers... This indicates the current and predicted dynamic obstacle occupancy status, while the layer... This representation is used to store time-invariant static obstacle information. It enables collision feasibility assessment through efficient array lookups, providing a computational foundation for forward-looking local planning.

[0079] The complete construction process of the unified spacetime tensor is as follows: Figure 3As shown:

[0080] Figure 3 (a) is a schematic diagram of the local occupation grid centered on the unmanned surface vessel. An initial two-dimensional occupation map is generated by projecting the point cloud of the lidar as the spatial basis of the spatiotemporal tensor.

[0081] Figure 3 (b) is a schematic diagram of short-term motion prediction of dynamic obstacles. Based on the dynamic target motion state detected by millimeter-wave radar, a constant velocity model is used to complete the position prediction of the next T time steps, and the shape of the obstacle is projected to each predicted position to construct a dynamic occupancy layer.

[0082] Figure 3 (c) is a schematic diagram of the hierarchical safety semantics of the spatiotemporal tensor. The physical safety boundary and the robust safety boundary are generated by two morphological dilations, respectively, to set the hierarchical safety margin for collision verification.

[0083] Figure 3 (d) is a schematic diagram of the overall structure of the three-dimensional spatiotemporal tensor. The tensor is a three-dimensional structure of H×W×(T+2), in which layers 0~T store the predicted occupancy information of dynamic obstacles, and layer T+1 stores the information of static obstacles separately, realizing the unified modeling of dynamic and static obstacles.

[0084] Discretize the local map centered on the unmanned surface vessel into a single... The grid is used to generate an initial occupancy layer by projecting the LiDAR point cloud, which is used to represent surrounding obstacles. This initial occupancy map serves as a static layer. The foundation. Let the left boundary of the local map be... The upper boundary is For any point Its corresponding grid index The calculation is as follows:

[0085] (6)

[0086] in This represents the grid resolution. The resulting occupancy map serves as the spatial basis for constructing the spatiotemporal tensor. Millimeter-wave radar provides detection information for dynamic targets, including their location. ,speed and surrounding dimensions To avoid redundant representations caused by two types of sensors simultaneously detecting the same obstacle, a dynamic-static separation step is performed. For each detected dynamic target, its currently occupied area is removed from the LiDAR occupancy map and assigned to the current dynamic layer. ), thus making the static layer ( Only static obstacle information is retained.

[0087] A constant velocity model is used to predict the motion of each dynamic target. For an initial position of... Speed ​​is The goal, in the first The predicted location at each time step is:

[0088] (7)

[0089] Subsequently, the obstacle shapes extracted at the current moment are projected onto these predicted locations to construct the future dynamic layer. (and maintain the same bounding size throughout the prediction time domain). .

[0090] The predicted area occupied by target b at time step k is denoted as This represents the set of grid cells occupied by the target. The dynamic occupation layer is constructed as follows:

[0091] (8)

[0092] The specific meaning of the above formula is: traverse each obstacle b in the set of all obstacles B, and check whether the grid point (i,j) is located in the predicted occupied area of ​​the obstacle at time k. Inside. If this condition is true for any obstacle b, then this mesh cell is marked as "occupied" (the value should be 1), where, For the traversal form of the "logical OR" operation, II[·] indicates a value of 1 when the condition within the parentheses is true, and 0 otherwise. This processing ensures consistency in the occupancy representation when predicted occupancy regions overlap.

[0093] Based on the occupancy representation, a hierarchical safety margin is introduced through morphological dilation. The first dilation generates a physical safety boundary to address the uncertainty of obstacle position caused by sensor errors and inaccurate positioning, and serves as a strict collision constraint; the second dilation further introduces a robust safety boundary to compensate for uncertainties in motion prediction and trajectory tracking, especially for dynamic obstacles.

[0094] Let M (0) This represents the occupied tensor after dynamic projection. The first stage of expansion generates the physical safety boundary:

[0095] (9)

[0096] in, This indicates the structural elements used based on the uncertainty characteristics of the static and dynamic layers. Indicates the use of structural elements Perform morphological dilation on the input image (or matrix), M (1)This represents the result after the expansion operation of equation (9).

[0097] Subsequently, a second expansion is applied to it uniformly to generate additional robust safety boundaries:

[0098] (10)

[0099] in, Represents a structure element used for robust safety extensions. Indicates the use of structural elements Perform morphological dilation on the input image (or matrix), M (2) This represents the result after the expansion operation according to equation (10). Since M... (1) M (2) The final tensor M is defined as:

[0100] (11)

[0101] The following semantic values ​​were obtained:

[0102] (12)

[0103] The constructed spatiotemporal tensor serves as the collision query interface for the local planner. This is achieved by querying the collision query corresponding to the location... With time step tensor unit and combined with static layer It assesses collision risks, thereby achieving collision detection with constant time complexity and efficient real-time planning.

[0104] 3.3. Task-First Local Planning Framework

[0105] This embodiment proposes a task-priority local planner that maintains temporal consistency of the time-parameterized task trajectory while generating collision-free local paths. The planner operates within a finite prediction time domain, making decisions based on the spatiotemporal tensor representation of the current ship state and environment. Replanning is triggered only when tensor query results indicate potential occupancy on the future reference trajectory; otherwise, the ship continues trajectory tracking without local intervention. The overall planning process is formally described in Algorithm 1, and its conceptual diagram is shown below. Figure 4 As shown: The planner uses the original reference trajectory as a benchmark and triggers replanning based on the collision risk pre-verification results of the spatiotemporal tensor. Under the collision-free constraints that satisfy the physical safety boundary and robust safety boundary, it selects the optimal action with the smallest deviation from the navigation direction from the candidate action set, generating a local collision avoidance path with minimal interference to the task progress.

[0106] The planner constructs the local path P as a sequence of predicted states:

[0107] (13)

[0108] It is the predicted state sequence that constitutes path P.

[0109] The planning time domain length N is defined as follows:

[0110] (14)

[0111] in, The maximum number of prediction steps considered by the planarizer. This represents the total number of points in the reference trajectory.

[0112] The planner performs a tree search on a discrete action space defined by the yaw rate command. This action space R is defined as:

[0113] (15)

[0114] in, This represents the discrete bow angular velocity command sampled within the allowed bow angular velocity range, corresponding to the i-th candidate bow angular velocity command in the discrete action space R. In each planning step, the current (k-th step) bow angular velocity is used... A set of local candidate actions is constructed as a reference. The index of the command closest to the current bow roll rate is determined by the following formula:

[0115] (16)

[0116] Among them, operators This indicates the search for expressions that make the subsequent expression... The index i that reaches the minimum value is then denoted as . .

[0117] This constructs an adjacent set of candidate actions R. cand as follows:

[0118] (17)

[0119] The above formula is based on the reference index found by formula (16). Construct a local candidate set containing only three actions. ,in, This indicates the discrete action closest to the current velocity. Decrementing or incrementing the index by one corresponds to adjusting the angular velocity in the negative direction and in the positive direction, respectively. This represents the intersection of the set with the complete discrete action space R, ensuring the validity of the set's index.

[0120] Formulas (16) and (17) together define a local search strategy. The planner does not evaluate all possible actions, but only considers a few options that are "fine-tuned based on the current motion state" (turn left slightly, hold, turn right slightly). This design greatly reduces the search space and improves real-time computation efficiency. At the same time, it meets the physical constraints of large ship motion inertia and the need for continuous and smooth control commands, which is an important guarantee for the algorithm to achieve real-time planning.

[0121] For each candidate bow roll rate command The planner performs a forward prediction based on the ship's kinematics model. Guidance reference. Obtained from the guidance law described in Chapter 1.2. The subsequent state is calculated as follows:

[0122]

[0123]

[0124] (18)

[0125]

[0126]

[0127] in, and These represent the longitudinal acceleration and deceleration limits of the ship, respectively. Xu's maximum longitudinal velocity.

[0128] To ensure collision avoidance safety, the feasibility of each predicted state is verified using the spatiotemporal tensor representation of the environment. The set of safe actions is R. safe Defined as:

[0129] (19)

[0130] Among all candidate actions that satisfy the safety constraints, the planner selects the action that best aligns with the desired heading direction. Its evaluation function is defined as:

[0131] (20)

[0132] Therefore, the optimal heading angular velocity is obtained according to the following formula:

[0133] (twenty one)

[0134] The aforementioned single-step decision-making process is embedded in a multi-step search framework, generating a feasible local path by progressively expanding the state tree. A depth-first search strategy with backtracking is used to progressively expand the planned path. When a branch cannot be further expanded within the prediction time domain, the planner backtracks to the previous state and explores other candidate actions.

[0135] Once a feasible local path is obtained, it is passed to the controller for execution. To maintain task continuity, the planner employs a trajectory regression mechanism. The ship returns to the original time-parameterized reference trajectory when the following conditions are met:

[0136] (twenty two)

[0137] in, This indicates the lateral distance between the ship and the reference trajectory. This is an adjustable scaling factor used to determine the allowable deviation range for trajectory regression. The unobstructed occupancy of the reference trajectory is verified by performing a tensor query on the reference trajectory over the next *n* time steps. When both of the above conditions are met, the planner terminates local replanning and resumes trajectory tracking.

[0138] Through the above process, the planner ensures that collision-free local paths are generated while maintaining consistency with the time-parameterized mission trajectory, thereby ensuring both navigation safety and mission continuity.

[0139]

[0140] 4.4. Verification Experiment

[0141] This section evaluates the proposed framework through a series of simulation experiments. The experiments aim to assess its computational efficiency, collision avoidance performance, and trajectory recovery capability under different geometric trajectories and obstacle configurations. First, the contribution of the spatiotemporal tensor is analyzed through ablation experiments; then, three typical navigation scenarios with increasing complexity are selected to verify the overall effectiveness of the proposed planning framework.

[0142] 4.1. Ablation Experiment

[0143] The contribution of the spatiotemporal tensor to the overall planning efficiency was analyzed through ablation experiments. Two collision detection strategies were compared: the proposed grid-based spatiotemporal query method and the baseline method based on real-time geometric computation. All experiments were conducted under the same environmental settings and system parameters, with consistent guidance laws, controllers, search strategies, action spaces, prediction time domains, and reference trajectories. The only difference lay in the collision detection module.

[0144] Table 1 shows the average and maximum planning times per step for each scenario. The results demonstrate that the proposed spatiotemporal representation significantly improves computational efficiency in all scenarios. The observed speedup ranges from 5.1 to 140.3, indicating that the discretized spatiotemporal query transforms collision detection into a constant-time query operation suitable for real-time planning.

[0145] Table 1 Efficiency Comparison

[0146]

[0147] 4.2. Simulation Experiment

[0148] This embodiment constructs three simulation scenarios to evaluate the performance of the proposed framework on task trajectories with different geometric structures, including straight-line trajectories, circular trajectories, and complex S-shaped trajectories. These scenarios correspond to progressively more complex geometric conditions in the obstacle avoidance problem during trajectory tracking. Detailed initial conditions for each scenario are shown in Table 2, and the quantitative results for all scenarios are summarized in Table 3.

[0149] All indicators were calculated within a time interval of t>50s to eliminate the influence of deviations between the ship and the reference trajectory in the initial stage, since the ship's initial position deviates slightly from the reference trajectory.

[0150] 4.2.1. Scenario 1: The task trajectory is a straight line containing multiple obstacles.

[0151] In this scenario, the global task trajectory is a straight path from the initial position to the target position, serving as a reference trajectory in the case of unobstructed access.

[0152] The simulation results of collision avoidance performance in this scenario are as follows: Figure 5 As shown:

[0153] Figure 5 (a) is a schematic diagram comparing the actual navigation trajectory of the ship with the reference straight trajectory. It can be seen that the ship completed the collision avoidance in the obstacle area by temporarily deviating laterally, and smoothly returned to the reference trajectory after the conflict was resolved.

[0154] Figure 5 (b) is the curve of the change in bow roll rate during navigation, corresponding to the heading adjustment maneuver during the collision avoidance process;

[0155] Figure 5 (c) is the curve of longitudinal velocity change during navigation. It can be seen that the collision avoidance phase compensates for the length of the detour path by a short-term acceleration, thereby reducing the time delay.

[0156] Figure 5 (d) is the curve showing the change in track error during navigation;

[0157] Figure 5(e) is the curve of the change of lateral error during navigation. Both types of errors converge to zero quickly after the collision avoidance ends, which verifies the mission time consistency maintenance capability of the proposed method.

[0158] When a potential conflict is predicted, the local planner adjusts the bow roll rate ( Figure 5 (b) and adjust the longitudinal speed ( Figure 5 (c) Generates a collision avoidance maneuver. During the bypass phase, the guidance law temporarily increases the commanded speed above the reference value to compensate for the additional path length and reduce time delay. Once the ship has bypassed the obstacle and returned to the reference trajectory, both the bow roll rate and longitudinal speed gradually return to the speed values ​​of the mission trajectory.

[0159] As shown in Table 3, the maximum values ​​of lateral error and track-side error reached 24.32m and 5.55m respectively, corresponding to temporary deviations caused to avoid obstacles. Figure 5 As shown in Figure (d) and Figure 5(e), after the collision avoidance maneuver ends, both types of errors gradually converge to zero, indicating that the ship can not only return to the reference path, but also recover the mission progress of the time-parameterized mission trajectory.

[0160] 4.2.2. Scenario 2: The task trajectory is a circular trajectory containing multiple obstacles.

[0161] In this scenario, the mission trajectory is set as a circular time-parameterized reference trajectory centered at (300,300)m with a radius of 200m. The ship needs to respond to multiple static and dynamic obstacles along this continuous curvature reference trajectory.

[0162] The simulation results of collision avoidance performance in this scenario are as follows: Figure 6 As shown:

[0163] Figure 6 (a) is a schematic diagram comparing the actual navigation trajectory of the ship with the reference circular trajectory. It can be seen that the ship can stably complete multi-obstacle collision avoidance while tracking along a continuous curvature circular trajectory.

[0164] Figure 6 (b) is the curve of the change in bow roll rate during navigation, corresponding to the heading adjustment during circular trajectory tracking and collision avoidance;

[0165] Figure 6 (c) is the curve of longitudinal velocity change during navigation. During the collision avoidance phase, speed adjustment is used to ensure safety and mission progress.

[0166] Figure 6 (d) is the curve showing the change in lateral error during navigation;

[0167] Figure 6(e) is the curve of the change of the error along the track during the navigation process. After each collision avoidance maneuver, both types of errors converged rapidly, which verifies the ability of the method to maintain mission consistency under continuous curvature trajectory.

[0168] like Figure 6 (b) and Figure 6 As shown in (c), both the bow roll rate and longitudinal velocity changed significantly during collision avoidance on the circular trajectory. When a potential conflict was predicted, the ship temporarily deviated from the circular reference trajectory by adjusting its course and speed, thereby generating a feasible path to avoid the obstacle. After passing through the conflict zone, the control input gradually returned to tracking mode, causing the ship to rejoin the circular reference trajectory. Quantitative results are summarized in Table 3. Due to the continuous curvature of the reference trajectory and the presence of collision avoidance maneuvers, the tracking deviation increased compared to the straight trajectory scenario. Nevertheless, the ship remained in a collision-free state throughout the mission. Figure 6 (d) and Figure 6 As shown in (e), after each collision avoidance maneuver, both the lateral error and the along-track error decreased again, indicating that the ship gradually returned to the reference trajectory and restored consistency with the time-parameterized mission trajectory.

[0169] 4.2.3. Scenario 3: The task trajectory is a complex trajectory containing multiple obstacles.

[0170] In Scenario 3, the reference mission trajectory is a complex S-shaped path with multiple curvature changes. This setting creates a more challenging navigation environment, where the ship must navigate obstacles while continuously changing course.

[0171] The simulation results of collision avoidance performance in this scenario are as follows: Figure 7 As shown, the motion results of the proposed framework are presented under the presence of multiple static and dynamic obstacles:

[0172] Figure 7 (a) is a schematic diagram comparing the actual navigation trajectory of the ship with the complex S-shaped reference trajectory. It can be seen that the ship can stably respond to multiple obstacle interactions and complete collision avoidance during the complex trajectory tracking process with multiple curvature changes.

[0173] Figure 7 (b) is the curve of the change in bow roll rate during navigation, corresponding to the heading adjustment during complex trajectory tracking and collision avoidance;

[0174] Figure 7 (c) is the curve of longitudinal velocity change during navigation, showing the characteristics of short-term acceleration during the collision avoidance phase and deceleration during the return phase, thus achieving resynchronization of mission progress.

[0175] Figure 7 (d) is the curve showing the change in lateral error during navigation;

[0176] Figure 7 (e) is the curve of the change of the error along the track during the navigation process. After each collision avoidance maneuver, both types of errors converge to zero, which verifies the effectiveness of the method in complex trajectory scenarios.

[0177] As the vessel advanced along the path, multiple obstacle interactions triggered temporary collision avoidance maneuvers. During these maneuvers, the vessel deviated from its trajectory to safely avoid nearby obstacles and returned to the reference path after passing through the conflict zone. These maneuvers were characterized by significant changes in both longitudinal velocity and bow roll rate. In particular, the longitudinal velocity profile exhibited brief acceleration and deceleration phases, corresponding to the collision avoidance process and the subsequent recovery of mission progress.

[0178] The quantitative results summarized in Table 3 show that the ship remained in a collision-free state throughout the entire scenario. The root mean square value of the lateral deviation was 7.72 m, with a maximum deviation of 35.41 m, mainly occurring during the temporary deviations caused by the ship's S-shaped path to avoid obstacles. After each collision avoidance maneuver, both the lateral deviation and the along-track deviation converged to zero again, indicating that the ship was able to return to the planned path and resume the expected mission progress.

[0179] 4.3. Sea Trial Results

[0180] like Figure 8 As shown, the sea trial verified in this embodiment uses an experimental vessel that is 4.5 meters long, 2 meters wide, and has a displacement of approximately 2 tons. The vessel is equipped with dual propulsion units, achieving a maximum rotational speed of 800 RPM and a maximum speed of approximately 5 knots. Due to mechanical constraints, the rudder angle range is 30°, and the maximum rotational speed is 6° / s. These constraints create relatively strict actuator limitations, thus providing a real-world platform for verifying the effectiveness and practicality of the proposed algorithm. The unmanned surface vessel is equipped with an inertial navigation system for real-time measurement of the vessel's attitude and motion, and also features lidar and millimeter-wave radar for obstacle detection, providing reliable environmental information for navigation.

[0181] To verify the feasibility and effectiveness of the proposed algorithm, the experiment focused on autonomous collision avoidance under a time-parameterized straight-line trajectory. Three typical encounter scenarios were designed, including face-to-face, intersection, and overtaking, corresponding to typical ship encounter patterns in actual navigation. The schematic layout of the experimental setup is shown in Table 4.

[0182] Table 2 shows the initial information for the three scenarios. In the figure, TS (Target Ship) refers to the target ship.

[0183]

[0184] Table 3. Collision avoidance simulation results (after 50 seconds)

[0185]

[0186] in, This represents the root mean square value of the lateral error. This represents the maximum value of the error along the track. This is the root mean square value of the error along the track. This represents the maximum value of the lateral error.

[0187] Table 4. Sea Experiment Scenarios

[0188]

[0189] It should be noted that, unlike the simulation results, some residual error along the track still exists after the collision avoidance maneuver in the actual ship test. This phenomenon is mainly attributed to the execution constraints of the actual system, including propulsion response lag, actuator saturation, and environmental disturbances.

[0190] 4.3.1. Scenario 1: Encounter

[0191] This experiment investigated the behavior of the proposed framework in an encounter scenario, where the in-ship (OS) navigates along a pre-defined straight mission trajectory while an oncoming ship approaches from the opposite direction. When the target ship enters the predicted conflict zone, the in-ship performs a collision avoidance maneuver by introducing a lateral offset on the reference path.

[0192] The results of the sea trial under the encounter scenario are as follows: Figure 9 As shown:

[0193] Figure 9 (a) is a schematic diagram comparing the actual navigation trajectory of the ship with the reference straight trajectory. It can be seen that the ship completed the safe collision avoidance of the encountering ship by temporarily shifting laterally.

[0194] Figure 9 (b) is the curve of the change in bow roll rate during navigation, corresponding to the heading adjustment maneuver during the collision avoidance process;

[0195] Figure 9 (c) is the curve of longitudinal speed change during navigation, and collision avoidance safety is ensured by adjusting the speed appropriately;

[0196] Figure 9 (d) is the curve of the change in lateral error during navigation, which reaches its peak during the collision avoidance phase and then gradually decreases as the ship returns to the reference trajectory.

[0197] Figure 9 (e) is the curve showing the change in track error during navigation, which verifies the ability of this method to maintain mission time consistency in a real marine environment.

[0198] This response is accompanied by a significant change in the bow roll rate (e.g. Figure 9As shown in (b), while making appropriate adjustments to the longitudinal velocity (such as...). Figure 9 (As shown in (c)). After the approaching vessel passed, the vessel gradually returned to the reference path. The process of deviation change is as follows. Figure 9 (d) and Figure 9 As shown in (e). Specifically, the lateral deviation peaks during the collision avoidance maneuver and then gradually decreases as the vessel turns back to its nominal path. According to Table 5, the root mean square value of the lateral deviation is 3.56 m, with a maximum value of 15.32 m; the root mean square value of the deviation along the track is 4.64 m, with a maximum value of 15.94 m. The results indicate that the vessel successfully avoided the oncoming vessel, and the deviation gradually decreased after the encounter ended.

[0199] Table 5 Results of the sea trials

[0200]

[0201] 4.3.2. Scenario Two: Crossover

[0202] This experiment investigated the behavior of the proposed framework in a cross-encounter scenario, where the main vessel (OS) navigates along a straight mission course while another vessel approaches from the port side and crosses the course. When the target vessel enters the predicted conflict zone, the main vessel performs collision avoidance maneuvers by deviating from the nominal course.

[0203] The results of the sea trial in this cross-encounter scenario are as follows: Figure 10 As shown:

[0204] Figure 10 (a) is a schematic diagram comparing the actual navigation trajectory of the ship with the reference straight trajectory. It can be seen that the ship safely avoided the target ship crossing the route by temporarily deviating from the reference route.

[0205] Figure 10 (b) is the curve showing the change in bow roll rate during navigation, corresponding to the heading adjustment during collision avoidance;

[0206] Figure 10 (c) is the curve of longitudinal velocity change during navigation, which is compensated for the path increment caused by collision avoidance by short-term speed increase;

[0207] Figure 10 (d) is the curve of the change in lateral error during navigation, which gradually converges after the collision avoidance peak as the ship returns to the reference trajectory.

[0208] Figure 10 (e) is the curve showing the change in track error during navigation, which verifies the adaptability of this method to the real marine environment in the scenario of cross-encounter.

[0209] This maneuver manifests as a change in the bow roll rate (e.g.) Figure 10(as shown in (b)) and short-term increases in longitudinal velocity (such as...) Figure 10 (As shown in (c)). After the crossing vessel passes through the interaction area, the main vessel gradually turns and returns to the reference path. Figure 10 (d) and Figure 10 As shown in (e), the lateral deviation reaches its peak during the collision avoidance process and then gradually decreases as the ship returns to its nominal path.

[0210] According to Table 5, the root mean square value of the lateral deviation is 6.02m, and the maximum value is 23.66m; the root mean square value of the deviation along the track is 4.35m, and the maximum value is 15.37m.

[0211] 4.3.3. Scenario 3: Overtaking

[0212] The experiment evaluated the performance of the proposed framework in an overtaking scenario where the ship (OS) encounters a slower vessel ahead on the same path.

[0213] The results of the sea trial in this overtaking and encounter scenario are as follows: Figure 11 As shown:

[0214] Figure 11 (a) is a schematic diagram comparing the actual navigation trajectory of the ship with the reference straight trajectory. It can be seen that the ship safely overtook the slower ship ahead by temporary lateral maneuvering and smoothly returned to the reference trajectory after overtaking.

[0215] Figure 11 (b) is the curve of the change in bow roll rate during navigation, corresponding to the heading adjustment action during the overtaking process;

[0216] Figure 11 (c) is the curve of longitudinal speed change during navigation, and speed adjustment ensures safety and efficiency in the overtaking process;

[0217] Figure 11 (d) is the curve showing the change in lateral error during navigation;

[0218] Figure 11 (e) is the curve of the change of track error during the navigation process. After the maneuver ends, both types of errors gradually decrease, which verifies the effectiveness of the method in the overtaking scenario.

[0219] When the vessel approaches the target vessel from behind, the planner generates a temporary lateral maneuver to safely complete the overtake. This deviation process can be performed within [the specified timeframe]. Figure 11 As observed in (a), the vessel deviated from the reference trajectory before completing the overtaking maneuver and returned to the reference trajectory after the overtaking was completed. This maneuver was accompanied by a moderate change in the bow roll rate. Figure 11 (b) and slight adjustments to longitudinal speed ( Figure 11(c)). After completing the overtake and leaving the interaction area, the vessel smoothly converges back to the reference trajectory. The corresponding tracking performance is as follows: Figure 11 (d) and Figure 11 As shown in (e), both the track deviation and the lateral deviation gradually decreased after the maneuver ended. According to Table 5, the root mean square value of the lateral error was 4.63 m, with a maximum value of 15.42 m; the root mean square value of the track error was 3.58 m, with a maximum value of 10.94 m.

[0220] In summary, this proposal presents a mission-priority local planning framework for unmanned surface vessels (USVs) that maintains the continuity of the time-parameterized task while achieving collision avoidance. This framework integrates a unified spatiotemporal tensor representation, a local search strategy satisfying kinematic constraints, and a guidance law-based velocity adjustment mechanism to restore task progress. Collision detection is performed via tensor queries, enabling the planner to evaluate candidate motions while satisfying ship kinematic constraints. The planner triggers collision avoidance maneuvers only when a potential conflict is detected, and task progress is restored after the interaction ends.

[0221] Extensive simulations on straight lines, circles, and complex trajectories, as well as real-ship tests in encounter, intersection, and overtaking scenarios, demonstrate the effectiveness of the proposed framework. During collision avoidance, the ship experiences a temporary spatial deviation from the reference trajectory; once the conflict is resolved, the ship gradually returns to the reference trajectory and resumes the progress of the time-parameterized task.

[0222] Experimental results show that the proposed framework provides a practical solution to the collision avoidance problem in dynamic marine environments, while maintaining consistency with the predetermined task sequence.

[0223] Based on the same inventive concept, this invention also provides a computer device, comprising: one or more processors, and a memory for storing one or more computer programs; the programs include program instructions, and the processor executes the program instructions stored in the memory. The processor may be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. It is the computing and control core of the terminal, used to implement one or more instructions, specifically for loading and executing one or more instructions stored in a computer storage medium to implement the above-described method.

[0224] It should be further explained that, based on the same inventive concept, the present invention also provides a computer storage medium storing a computer program, which, when executed by a processor, performs the above-described method. This storage medium can be any combination of one or more computer-readable media. A computer-readable medium can be a computer-readable signal medium or a computer-readable storage medium. A computer-readable storage medium can be, for example, but not limited to, an electrical, magnetic, optical, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of computer-readable storage media (a non-exhaustive list) include: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In the present invention, a computer-readable storage medium can be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, apparatus, or device.

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

[0226] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention in any other way. Any person skilled in the art may make changes or modifications to the above-disclosed technical content to create equivalent embodiments. However, any simple modifications, equivalent changes, and modifications made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the protection scope of the present invention.

[0227] This invention is not limited to the preferred embodiment described above. Anyone inspired by this invention can derive other forms of spatiotemporal tensor task-priority local programming methods for unmanned surface vessels. All equivalent variations and modifications made within the scope of the claims of this invention shall fall within the scope of this invention.

Claims

1. A spatiotemporal tensor task-priority local planning method for unmanned surface vessels, comprising acquiring the current motion state of the unmanned surface vessel, a preset time-parameterized reference trajectory, and environmental perception data collected by onboard sensors, characterized in that, It also includes the following steps: Step 1: Construct a unified three-dimensional spatiotemporal tensor based on environmental perception data; the three-dimensional spatiotemporal tensor encodes static obstacle information and future predicted occupancy information of dynamic obstacles in an integrated manner, supporting collision feasibility queries with constant time complexity for any combination of position and time. Step 2: Generate guidance reference information containing the desired heading based on the time-parameterized reference trajectory; Step 3: Generate a set of candidate control actions based on the current motion state, perform collision constraint verification on the candidate control actions based on the three-dimensional spatiotemporal tensor, and filter to obtain a set of safe actions that meet the collision-free constraints. In the set of safe actions, the optimal control action is selected with minimizing the deviation between the predicted ship's course and the desired course as the sole optimization objective. Step 4: Generate a local planning path based on the optimal control action to control the unmanned surface vessel's navigation.

2. The spatiotemporal tensor task-priority local programming method for unmanned surface vessels according to claim 1, characterized in that: The three-dimensional spatiotemporal tensor is a multi-layered three-dimensional structure formed by expanding a spatial grid centered on the unmanned surface vessel along the time dimension. The preceding layers store the dynamic obstacle occupancy status information in the current time and future prediction time domain, while the last layer stores static obstacle information.

3. The spatiotemporal tensor task-priority local programming method for unmanned surface vessels according to claim 2, characterized in that: When constructing a unified three-dimensional spatiotemporal tensor, two morphological dilation operations are performed on the initial occupied tensor to generate a physically safe boundary and a robustly safe boundary, respectively. Finally, the three-dimensional spatiotemporal tensor is assigned three-level semantic values, where the value 0 corresponds to the free region, the value 1 corresponds to the physically safe boundary, and the value 2 corresponds to the robustly safe boundary.

4. The spatiotemporal tensor task-priority local programming method for unmanned surface vessels according to claim 1, characterized in that: The candidate control actions are generated as follows: taking the current bow angular velocity of the unmanned surface vessel as a reference, in the preset discrete bow angular velocity action space, the reference action that is closest to the current bow angular velocity, as well as the previous and next adjacent actions of the reference action, are selected to form a set of candidate control actions.

5. The spatiotemporal tensor task-priority local programming method for unmanned surface vessels according to claim 3, characterized in that: The collision constraint verification rule is as follows: the candidate control action is determined to satisfy the no-collision constraint only when the spatiotemporal tensor values ​​of the layer storing dynamic obstacle occupancy state information and the layer storing static obstacle information at the corresponding prediction time of the predicted ship position corresponding to the candidate control action are both 0.

6. The spatiotemporal tensor task-priority local programming method for unmanned surface vessels according to claim 1, characterized in that: The evaluation function corresponding to the optimization objective is: in, Candidate actions The evaluation value, The predicted heading angle of the ship corresponding to the candidate maneuvers. The desired heading angle is given in the guidance reference information; the candidate action with the smallest evaluation value is selected as the optimal control action.

7. The spatiotemporal tensor task-priority local programming method for unmanned surface vessels according to claim 1, characterized in that: Step 3, the single-step decision-making process, is embedded in a depth-first multi-step search framework with backtracking: within the preset planning time domain, the state tree is gradually expanded to generate local planning paths. When a branch has no candidate action that satisfies the no-collision constraint, it backtracks to the previous state to explore other candidate actions until a complete feasible local planning path is generated or it backtracks to the initial state.

8. The spatiotemporal tensor task-priority local programming method for unmanned surface vessels according to claim 1, characterized in that: When performing forward prediction based on candidate control actions, longitudinal speed adjustment is performed simultaneously: when the expected speed in the guidance reference information is greater than the current speed, acceleration compensation is limited to the ship's maximum acceleration; when the expected speed is less than or equal to the current speed, deceleration adjustment is limited to the ship's maximum deceleration to compensate for the time delay generated during collision avoidance.

9. A spatiotemporal tensor task-priority local programming system for unmanned surface vessels, characterized in that, include: The data acquisition module is used to acquire the current motion state of the unmanned surface vessel, the preset time parameterized reference trajectory, and the environmental perception data collected by the shipborne sensors. The spatiotemporal tensor construction module is used to construct a unified three-dimensional spatiotemporal tensor based on environmental perception data. The three-dimensional spatiotemporal tensor encodes static obstacle information and future predicted occupancy information of dynamic obstacles in an integrated manner, supporting collision feasibility queries with constant time complexity for any combination of position and time. The guidance reference generation module is used to generate guidance reference information containing the desired heading based on the time-parameterized reference trajectory. The candidate action generation module is used to generate a set of candidate control actions based on the current motion state; The task priority decision module is used to perform collision constraint verification on candidate control actions based on the three-dimensional spatiotemporal tensor, filter to obtain a set of safe actions that meet the collision-free constraints, and select the optimal control action in the set of safe actions with minimizing the deviation between the predicted ship course and the desired course as the sole optimization objective. The navigation control module is used to generate a local planned path based on the optimal control actions to control the navigation of the unmanned surface vessel.

10. A spatiotemporal tensor task-priority local programming system for unmanned surface vessels according to claim 9, characterized in that, The spatiotemporal tensor construction module is specifically used to project the lidar point cloud to generate a two-dimensional occupancy grid centered on the unmanned surface vessel, use a constant velocity model to predict the motion of dynamic obstacles, and perform two morphological dilation operations on the initial occupancy tensor to generate a three-dimensional spatiotemporal tensor with three-level safety semantics.

Images