Unmanned ship integrated path planning method based on space-time state search and optimization

Abstract

The invention discloses an unmanned ship integrated path planning method based on space-time state search and optimization. The method comprises the following steps: constructing a unified space-time state space and dynamic model; performing forward search based on the space-time state space to generate a preliminary feasible trajectory; performing continuous space-time optimization on the preliminary trajectory to generate a smooth executable trajectory; performing online rolling execution and local re-planning; according to the method, a search space is lifted from a traditional two-dimensional geometric position to a four-dimensional space-time state space containing speed, course and time, and geometric accessibility, dynamic feasibility and space-time collision-free performance are synchronously considered in a unified framework. Different from an existing separated architecture that the path is planned first and then the speed is adjusted, an integrated track containing position, course, speed and time information is directly generated in the planning process, the problem that planning and execution are disjointed is fundamentally solved, and it is ensured that the planning result can be directly executed.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation and intelligent collision avoidance technology for unmanned surface vessels, specifically to an integrated path planning method for unmanned surface vessels based on spatiotemporal state search and optimization. Background Technology

[0002] Current dynamic path planning for unmanned surface vessels typically employs a separate architecture of "geometric global planning + velocity local obstacle avoidance." This approach decomposes the complex "spatiotemporal joint planning" problem into two relatively simple sub-problems, reducing the difficulty of real-time computation and making it easier to implement and verify in engineering. However, this two-stage framework, which separates "geometric path search" (such as A and D) from "velocity collision avoidance decision" (such as the VO series methods), faces significant engineering challenges in complex dynamic water environments. The planning-execution feasibility is disconnected: the upstream geometry searcher aims for the shortest path, while the downstream velocity obstacle avoidance system is constrained to zero collisions, resulting in a lack of coordinated optimization between the two. This often leads to the discovery of the "optimal" geometric path, which cannot be executed because there is no corresponding safe speed in the VO velocity space, forcing the USV to frequently stop or replan, resulting in discontinuous navigation.

[0003] Lack of dynamic feasibility: Existing search algorithms are performed on discrete grids or sampling points, and the paths they generate are often a series of broken lines. Even with post-processing smoothing, it is difficult to strictly meet the dynamic constraints inherent to USV, such as the minimum turning radius and the maximum rate of change of heading, so that the planning results "appear feasible, but are actually impossible to track".

[0004] Dynamic environment heuristic failure: In waters filled with dynamic obstacles, the heuristic function of the traditional A* algorithm relies solely on the static distance to the target, ignoring the future motion of moving obstacles. The direct consequence is that the USV is easily guided to narrow areas about to be blocked by obstacles, leading to unnecessary emergency avoidance, path oscillations, and even getting stuck in a local dead loop.

[0005] In summary, the core contradiction of current technology lies in the fact that the geometric optimality of the path, speed feasibility, dynamic executability, and dynamic risk perception are handled in multiple independent modules, lacking a unified modeling and optimization framework. This makes it difficult for the planning results to simultaneously meet the navigation requirements of unmanned surface vessels in spatial, temporal, and control dimensions, thus limiting the application scope and reliability of unmanned surface vessels in complex and dynamic waters.

[0006] Therefore, there is an urgent need for a planning method that can integrate spatiotemporal constraints, curvature constraints of dynamics, and risk fields of dynamic obstacles into a single model at the search layer. This would ensure the global consistency and executability of the planning results across spatial, temporal, and control dimensions from the source, and solve problems such as the disconnect between planning and execution, dynamic infeasibility, and insufficient prediction of dynamic risks in existing technologies. Summary of the Invention

[0007] This invention aims to overcome the above-mentioned deficiencies of the prior art and proposes an integrated path planning method for unmanned surface vessels (USVs) based on spatiotemporal state search and optimization. This method unifies the motion state (position, heading, velocity) of USVs with time and performs direct search within an extended spatiotemporal state space. It can simultaneously generate geometrically feasible, dynamically executable, and real-time collision avoidance integrated paths in complex dynamic aquatic environments, so that the generated paths fundamentally satisfy both dynamic constraints and spatiotemporal collision avoidance constraints.

[0008] To achieve the above objectives, the present invention adopts the following technical solution: An integrated path planning method for unmanned surface vessels based on spatiotemporal state search and optimization includes the following steps: Step S1: Construct a unified spatiotemporal state space and dynamics model; 1. Definition of spatiotemporal state space To achieve spatiotemporal joint planning, this invention extends the state of the unmanned surface vessel (USV) from the traditional two-dimensional planar position to a four-dimensional state vector including the time dimension. The extended state of the USV at time t is defined as follows: Among them, (x t , y t ) represents the planar coordinate position (in meters) of the unmanned surface vessel at time t, ψ t Represents the heading angle (unit: rad, range: [-π, π]), v t This represents the forward speed (unit: m / s); this state vector comprehensively covers the spatial position, direction of motion, and speed information of the unmanned surface vessel, laying the foundation for spatiotemporal joint modeling.

[0009] 2. Establishment of dynamic model Based on the above extended state, a simplified three-degree-of-freedom discrete kinematic model is established to accurately describe the state transition process of the unmanned surface vessel. The model expression is as follows:

[0010] Where, ω t The angular velocity (in rad / s) of the unmanned surface vessel determines the rate of change of its heading angle; a t Δt is the acceleration of the unmanned surface vessel (unit: m / s²), which determines the rate of change of velocity; Δt is the control period (unit: s), which is set according to the response speed of the unmanned surface vessel's control system and the frequency of dynamic changes in the environment, and is usually taken as 0.5~2.0s. Establish control input Its value range is determined by the performance of the unmanned surface vessel's physical actuators, that is... U admThis is the permissible control set. For example, for a conventional 5-meter class unmanned surface vessel, the permissible control set is typically a∈[−1,1]m / s², ω∈[−0.5,0.5]rad / s.

[0011] This dynamic model directly links control inputs and state transitions, and can naturally embed the dynamic constraints of the unmanned surface vessel in the state space, ensuring that the subsequently generated path conforms to the actual motion capabilities of the unmanned surface vessel.

[0012] Step S2: Perform a forward search based on the spatiotemporal state space to generate a preliminary feasible trajectory.

[0013] This invention employs an improved spatiotemporal A* search algorithm or state lattice search algorithm to search in the state space graph G(S, E), where each node searched is an extended state S. t Each edge E corresponds to a legal transition between two states, which is a short trajectory.

[0014] The search process starts from the initial state S start Starting with (x0, y0, ψ0, v0) (initial position, initial heading, initial velocity), a preliminary feasible trajectory is generated iteratively through the following sub-steps: Step S21 Node Expansion: For the optimal node St with the lowest cost in the current priority queue, in the permissible control set U... adm The method employs uniform sampling or heuristic sampling to select a set of control inputs u. t For example, consider seven typical action combinations: maintaining speed and heading, accelerating straight ahead, decelerating straight ahead, turning left, turning right, accelerating left, and decelerating right. Substitute each control input into the dynamic model established in step S1 for forward simulation to calculate the subsequent state S at time t+Δt. t+Δt .

[0015] During this process, since the control input is strictly limited to the allowable control set, the generated subsequent state naturally satisfies the dynamic boundary constraints such as the speed, acceleration, and angular velocity of the unmanned surface vessel, thus avoiding the problem that the subsequent path cannot be executed due to exceeding the motion capability.

[0016] Step S22: Spacetime Collision Detection To ensure the safety of state transitions, it is necessary to specify the generated state transition from S. t To S {t+Δt} The state transition edges (representing a short trajectory) undergo rigorous collision detection, including both static obstacle detection and dynamic obstacle detection. Static obstacle detection: First, a static occupied grid map is constructed. This map is generated by fusing pre-loaded high-precision electronic nautical charts (containing information on reefs, islands, shoals, and channel boundaries) with environmental data collected by real-time sensors (such as radar and sonar). The system detects whether state transition edges intersect with obstacle areas in the static occupied grid map. If they intersect, a collision is identified, and the state transition edge is invalid.

[0017] Specifically, to reduce the grid depression effect caused by complex coastlines, reefs, and dense shoals, and to explicitly introduce a safety buffer at the planning level, this invention geometrically reconstructs the static obstacle set based on a static grid map. First, the original static obstacle set obtained by fusing electronic charts and sensors is denoted as...

[0018] Each Q i Represented by the set of boundary points. For any two obstacles Q i Q j When the nearest distance is less than the preset merging threshold d merge When these points are considered as a single obstacle cluster, convex hull operations are performed on all boundary points within the cluster to obtain a static obstacle convex hull set.

[0019] Based on this, each convex hull is expanded outward according to a safe buffer distance to construct a buffer no-navigation zone.

[0020] This results in a new set of static obstacles.

[0021] During the spatiotemporal search, collision detection is no longer based on the original obstacle set O, but is uniformly applied to the buffer no-fly set. The process proceeds as long as the state transition edge connects to any B. kIntersection is considered a collision. This method replaces the jagged grid boundary with a continuous convex hull boundary, effectively eliminating "false slits" caused by concave areas, and explicitly encodes the safety buffer distance to ensure planning safety and stability in complex shoreline scenarios. Dynamic obstacle detection: The state of dynamic obstacles (such as other ships and floating objects) is collected in real time through devices such as Automatic Identification System (AIS), radar, and visual sensors to obtain information such as their position, speed, and heading. Assuming that the dynamic obstacle moves in a uniform linear motion within the prediction time window [t, t+Δt] (if the sensor can provide acceleration information, a uniform acceleration motion model can be used to improve prediction accuracy), its motion trajectory is extended into a "spatiotemporal tube," that is, a tubular region in three-dimensional space (x, y, time) composed of the obstacle's position, motion range, and time window. It detects whether the state transition edge intersects with this spatiotemporal tube. If they intersect, it is determined that there is a potential collision risk, and the state transition edge is invalid.

[0022] By using spatiotemporal collision detection, true spatiotemporal joint collision avoidance is achieved, ensuring that the unmanned surface vessel will not collide with static or dynamic obstacles when it arrives at the planned location at the planned time.

[0023] To achieve integrated modeling of static and dynamic obstacles in the spatiotemporal domain, this invention further constructs a unified spatiotemporal obstacle set. The buffered static obstacle set constructed in step S22 is expanded using a time outer product as follows:

[0024] Dynamic obstacles are constructed using a spatiotemporal tube based on a prediction model.

[0025] The final unified set of spacetime barriers is obtained:

[0026] During the search phase, it is determined whether the state transition edge should be added to the set. This enables unified collision avoidance judgment for both static and dynamic obstacles. t f It is the end of the planning and forecasting time window. Let be the spatial region occupied by the i-th dynamic obstacle at time t, and n be the number of dynamic obstacles.

[0027] Step S23 Cost Evaluation and Heuristic Guidance: For a safe successor node S' that passes collision detection, its total cost needs to be calculated to guide the search algorithm towards the optimal direction. The total cost f(S') consists of two parts: the actual cost and the heuristic cost, expressed as follows: f(S') = g(S') + λ×h(S'), Here, λ is a weighting factor (usually ranging from 1.0 to 2.0) used to balance the impact of actual costs and heuristic costs. Nodes are inserted into the priority queue according to their f values.

[0028] Actual cost g(S'): represents the cost from the initial state S start The cumulative cost up to the current successor node S', taking into account multiple optimization objectives such as travel time, energy consumption, and changes in control variables, is expressed as: g(S')=g(S t )+α×Δt+β×(v t+Δt -v t ) 2 +γ×(ω t ) 2 Where α, β, and γ are weighting coefficients, which can be adjusted according to task requirements (e.g., increasing the value of β for energy-saving priority tasks, and increasing the value of γ for stable navigation priority tasks); Δt is the time cost of the current control cycle; (v t+Δt -v t ) 2 The square of the change in velocity reflects the energy cost; (ω) t ) 2 It is the square of the angular velocity, reflecting the cost of control stability.

[0029] Heuristic cost h(S'): Used to guide the search algorithm toward the target region, while incorporating dynamic risk perception. It consists of the geometric distance term hgeo(S') and the spatiotemporal risk term hrisk(S'), and its expression is: h(S') = hgeo(S') + hrisk(S') The geometric distance term hgeo(S') represents the estimated geometric distance from the current node S' to the target region. To improve the accuracy of the heuristic function, an improved Euclidean distance considering the influence of ocean currents is used, and its expression is:

[0030] Where (x', y') are the position coordinates of node S', (x g ,y g ) represents the center coordinates of the target region, v curr Let Δt be the current ocean current velocity. curr For the duration of ocean current influence, v max This represents the maximum speed of the unmanned surface vessel. This expression fully considers the impact of ocean currents on travel distance and time, making the heuristic guidance more closely aligned with real-world environments.

[0031] h risk(S'): Spatiotemporal risk term. Used to assess the threat level of dynamic obstacles near node S', guiding the search algorithm to actively avoid high-risk spatiotemporal regions. Constructed based on the shortest predicted collision time (TCPA) and nearest encounter distance (DCPA) with the nearest dynamic obstacle, the expression is:

[0032] Where n is the number of dynamic obstacles; TCPA i DCPA is the shortest predicted collision time (in seconds) between the unmanned surface vessel (USV) and the i-th dynamic obstacle in state S'. i ε is the nearest encounter distance (in meters) between the unmanned surface vessel and the i-th dynamic obstacle; ε is the risk threshold (usually set to 0.1), when TCPA i When the value is greater than 1 / ε, the contribution of this term is zero, meaning the obstacle is considered to pose no significant threat; δ is a smoothing factor (usually set to 0.5) to avoid DCPA. i When it approaches zero, the term becomes infinite.

[0033] Step S24 Termination Condition When a state node S that meets the following objective conditions is found... goal Stop the search when: Location conditions: The position coordinates (x, y) of node Sgoal and the center coordinates (x, y) of the target region. g ,y g The distance between the unmanned surface vessel and the target is less than the set threshold (e.g., 2-5m, adjusted according to the size of the unmanned surface vessel and the accuracy requirements of the mission). Velocity condition: Node S goal The unmanned surface vessel's (USV) speed v is less than a set threshold (e.g., 0.2-0.5 m / s) to ensure that it reaches the target area smoothly.

[0034] After the search stops, from the target node S goal Backtracking to the initial state S start This yields a discrete spatiotemporal state sequence, which is a rough but dynamically feasible and collision-free preliminary trajectory.

[0035] Step S3: Perform continuous spatiotemporal optimization on the initial trajectory to generate a smooth, executable trajectory.

[0036] The initial trajectory obtained in step S2 is a discrete state sequence, which has problems such as curvature discontinuity and velocity fluctuation, and cannot be directly used for accurate tracking of unmanned surface vessels. Therefore, it is necessary to use this discrete state sequence as an initial guess to construct a spatiotemporal trajectory optimization problem to generate a smooth, accurate final trajectory γ(t) = (x(t), y(t), ψ(t), v(t)) that satisfies all constraints.

[0037] Step S31: Optimize variable parameterization: To achieve continuity and smoothness of the trajectory, the trajectory is parameterized as a piecewise polynomial (such as a cubic B-spline), with the control points of the polynomial used as optimization variables. The advantage of using B-spline curves for parameterization is that it allows for flexible changes in the trajectory shape by adjusting the position of the control points, while ensuring high-order continuity of the trajectory, and it is computationally efficient, making it suitable for real-time planning scenarios.

[0038] For example, the position trajectories x(t) and y(t) are parameterized using cubic uniform B-spline curves, and the expression is:

[0039] Among them, P x,i P y,i N represents the control points of the B-spline curve. i,3 Let ψ(t) be a cubic B-spline basis function, and k be the number of control points. Similarly, for the heading angle ψ(t) and velocity v... (t) Perform B-spline parameterization.

[0040] Step S32: Constructing the objective function: With the goal of generating smooth, efficient, and safe trajectories, a multi-objective optimization function is constructed, expressed as: Minimize the following performance metrics:

[0041] Among them, the first item Used to control the rate of change of path velocity, ensuring that the trajectory accelerates and decelerates smoothly within the limits allowed by the vehicle's dynamics; the second item Used to constrain the curvature of the trajectory, ensuring that the curvature of the generated path is continuous and does not exceed the maximum permissible curvature k. max This satisfies the unmanned surface vessel's heading and turning capabilities; the third item By constraining the trajectory using the potential field of obstacles, the unmanned surface vessel can maintain a safe distance from obstacles. For static obstacles, this invention uses the buffer no-navigation set constructed in step S22 to define the obstacle potential field. For any trajectory point... It reaches the k-th buffer no-fly zone The minimum distance is expressed as ; Then the obstacle potential field function can be constructed: ; in For a single barrier potential function, when When the time approaches zero, The time increases rapidly, thus naturally pushing the trajectory away from the buffer no-navigation zone during the optimization process, achieving consistent modeling of static obstacles in the search and optimization layers; To maintain consistency between the search layer and the optimization layer, this invention constructs a distance potential field based on a unified spatiotemporal obstacle set. Definition:

[0042] The potential field is written as:

[0043] when The time increases dramatically, thus driving the optimized trajectory away from the no-fly zone, achieving consistency between obstacle modeling in search and optimization.

[0044] Step S33 Constraints: Dynamic constraints: The unmanned surface vessel's speed, acceleration, and angular velocity must be within the limits allowed by the physical actuators, i.e.: v min ≤ v(t) ≤ v max |ω(t)| ≤ ω max |a(t)| ≤ a max Among them, v min v is the minimum speed of the unmanned surface vessel (typically taken as 0.1-0.3 m / s). max a max ω max The maximum speed, maximum acceleration, and maximum angular velocity of the unmanned surface vessel are determined by hardware performance.

[0045] Curvature continuity constraint: B-spline parameterization ensures the continuity of the curvature κ(t) and its rate of change of the trajectory, and |κ(t)| ≤ κ. max κ max The maximum permissible curvature is determined by the minimum turning radius of the unmanned surface vessel. R min This is the minimum turning radius, fundamentally satisfying the USV minimum turning radius limit.

[0046] Collision avoidance constraint: For all obstacles i and time t, distance(γ(t), Obstacle i(t) ) ≥ d safe Obstacle i(t) It is the time-varying position of the dynamic obstacle, d safe It is a safe distance.

[0047] Step S34: Optimization problem solution: The aforementioned optimization objective and constraints are formalized as a nonlinear programming problem, which is solved using efficient numerical optimization methods such as Sequential Quadratic Programming (SQP) or the interior-point method. To improve real-time performance, mature optimization libraries such as Ceres Solver and IPOPT can be used for solving the problem. The optimal trajectory γ*(t) that satisfies all constraints is obtained through iterative optimization. This trajectory features continuous curvature, smooth velocity, and sufficient safety distance, and can be directly used for precise tracking and control of unmanned surface vessels.

[0048] Since the search phase uses an admissible control set for forward propagation, the generated discrete trajectory naturally satisfies the dynamics and control reachability of the unmanned surface vessel (USV). The optimization phase only performs continuity and smoothing on this trajectory without changing its controllability, thus ensuring that the final trajectory belongs to the trackable trajectory manifold of the USV, achieving an integrated closed loop between the planning and execution layers.

[0049] Step S4: Online rolling execution and local replanning.

[0050] Step S41, Trajectory Tracking Execution The lower-level controller (such as a PID controller or a model predictive controller, MPC) receives the optimized trajectory γ×(t) generated in step S3, tracks the position, heading and speed of the unmanned surface vessel in real time, and adjusts the control input according to the deviation between the actual state and the trajectory to ensure that the unmanned surface vessel sails accurately according to the optimized trajectory.

[0051] Step S42, Perception Update: In each planning cycle (typically 2-5 seconds, set in conjunction with the control cycle Δt), the environmental information is updated using the multi-sensor fusion system (radar, sonar, AIS, visual sensors, GPS, etc.) onboard the unmanned surface vessel, including: Correction of static obstacle maps (e.g., discovery of reefs not marked on electronic charts); Updates on the status of dynamic obstacles (such as changes in the speed and course of other vessels); Accurate estimation of the unmanned surface vessel's own state (such as position, heading, and speed obtained through GPS+IMU fusion).

[0052] Step S43, Local Replanning Triggering Conditions A new, unexpected obstacle is detected, and this obstacle is located in front of the current execution trajectory, which may lead to a collision; The actual motion state of the dynamic obstacle deviates significantly from the predicted model, and the collision avoidance constraints of the original trajectory are no longer satisfied. The unmanned surface vessel's trajectory tracking deviation exceeds a set threshold (e.g., 1-2m) due to external interference (such as strong winds or ocean currents), and cannot be corrected by the lower-level controller. The mission objective has changed (e.g., a new search and rescue point has been added temporarily).

[0053] Step S44: Local replanning execution Starting from the current actual state of the unmanned surface vessel, repeat steps S2-S3 within a local spatiotemporal window (the time window is usually 10-30 seconds in the future, and the spatial window is usually an area with a radius of 50-100m centered on the current position). However, the search range and time are limited to the current local spatiotemporal window. Quickly generate a local trajectory to avoid new risks and smoothly connect it to the global target.

[0054] Compared with the prior art, the present invention has the following significant advantages: This invention truly achieves integrated planning: by elevating the search space from the traditional two-dimensional geometric location to a four-dimensional spatiotemporal state space that includes velocity, heading, and time, it simultaneously considers geometric reachability, dynamic feasibility, and spatiotemporal collision-free behavior within a unified framework. Unlike existing separate architectures that "plan the path first and then adjust the velocity," this invention's planning process directly generates an integrated trajectory containing position, heading, velocity, and time information, fundamentally solving the problem of the disconnect between planning and execution, and ensuring that the planning results are directly executable.

[0055] The generated path is inherently feasible: Since each state transition during the search process is generated based on the dynamic model of the unmanned surface vessel (USV) and strictly follows the allowable range of control inputs, the generated preliminary path naturally satisfies the dynamic constraints of the USV, such as minimum turning radius, maximum acceleration, and maximum angular velocity. Unlike existing technologies, there is no need for additional post-processing to impose constraints after path generation, avoiding path conflicts and execution failures that may result from post-processing and improving the reliability of the planning results.

[0056] It features forward-looking intelligent collision avoidance: By using spatiotemporal collision detection technology, the trajectory of dynamic obstacles is extended into a three-dimensional spatiotemporal tube, enabling accurate prediction of future collision risks. Simultaneously, by incorporating spatiotemporal risk terms into a heuristic function, the search algorithm is guided to proactively avoid high-risk areas early in the decision-making process, avoiding the problems of traditional methods that "hesitate" or get stuck in local loops near obstacles. For example, in busy port waterways, this invention can predict the navigation trajectories of other vessels in advance and plan a smooth path away from conflict zones, significantly improving navigation safety.

[0057] Balancing global optimization with local flexibility: "Global spatiotemporal state search" provides the unmanned surface vessel (USV) with target-oriented global approximation, ensuring overall efficiency of the navigation path; "back-end continuous optimization" guarantees trajectory smoothness and accuracy through polynomial parameterization and multi-objective optimization; and "online local replanning" endows the system with strong robustness in the face of environmental uncertainties, enabling rapid response to emergencies. The organic combination of these three elements allows the USV to efficiently navigate towards its target while flexibly avoiding risks in complex dynamic environments, achieving a balance of safety, smoothness, and efficiency. Detailed Implementation

[0058] The present invention will be further described in detail below with reference to embodiments. This embodiment takes a 5-meter-long USV as the implementation object, and its hardware parameters and mission scenario are as follows: Maximum speed v max =3m / s, minimum sailing speed v min =0.2m / s; Maximum acceleration a max =1m / s², maximum deceleration a min =-1m / s²; Maximum angular velocity ω max =0.5rad / s, minimum turning radius R min =v max / ω max =6m; Equipped with sensors: GPS (positioning accuracy ±1m), IMU (attitude measurement accuracy ±0.1°), radar (detection range 0-500m, detection accuracy ±0.5m), AIS (receiving status information from other ships), and sonar (detecting underwater static obstacles). Control system: Embedded processor (2.0GHz), control cycle Δt = 1.0s, planning cycle = 2.0s.

[0059] The mission scenario is a busy nearshore waterway, containing the following environmental elements: Static obstacles: 3 fixed islands (radii of 10m, 15m, and 8m respectively) and 2 shallow waters (ranges of 20m×30m and 15m×25m respectively). Dynamic obstacles: 4 moving vessels (with speeds of 2m / s, 1.5m / s, 2.5m / s, and 1.8m / s respectively, whose headings change randomly but maintain uniform linear motion); Mission objective: The unmanned surface vessel (USV) starts from the starting point (0m, 0m), with a heading of 0° and an initial speed of 1m / s. It must smoothly reach the target area (100m, 80m) while avoiding all obstacles. The target area has a radius of 3m, and the speed upon arrival must be ≤0.3m / s.

[0060] Specific implementation process for each step: Step S1: Construction of spatiotemporal state space and dynamic model Define state S t =(x,y,ψ,v). The control input is constrained as follows: a ∈ [-1, 1] m / s², ω ∈ [-0.5,0.5] rad / s. The discretization time step is Δt = 1.0 s.

[0061] Dynamics model: The three-degree-of-freedom discrete kinematics model defined in the invention is directly embedded in the control system.

[0062] Step S2: Forward search of spatiotemporal state Search algorithm: An improved A* algorithm is used. Open sets use priority queues to store nodes.

[0063] Node expansion: For node S t Within the permissible control set, seven control input combinations are sampled: (0, 0) (hold), (0.5, 0) (accelerate), (-0.5, 0) (decelerate), (0, 0.3) (turn right), (0, -0.3) (turn left), (0.5, 0.3) (accelerate right turn), and (-0.5, -0.3) (decelerate left turn). Successor nodes are generated through forward simulation using the dynamic model. Collision detection: Static obstacle map: generated by integrating high-precision electronic nautical charts (including island and shoal information) with real-time radar and sonar data, with a grid resolution of 1m×1m; Dynamic obstacle spacetime tube: The position, speed, and heading of four ships are obtained through AIS and radar. Assuming that they are moving in uniform linear motion within the predicted time window [t, t+1.0s], a spacetime tube is constructed (radius is the ship size + safety margin = 5m). Cost function parameters: Actual cost weights: α=1.0, β=0.5, γ=0.3; Weighting factor λ = 1.2; Considering the absence of ocean currents, v max =3m / s, geometric distance term:

[0064] Spatiotemporal risk items:

[0065] Among them, TCPA i and DCPA i Calculated using Automatic Identification System (AIS) and radar data; Termination condition: |(x,y)-(100,80)|<3m and v<0.3m / s.

[0066] Step S3: Continuous Spatiotemporal Optimization Trajectory parameterization: The position (x(t), y(t)), heading ψ(t), and velocity v(t) are parameterized using cubic uniform B-spline curves. The number of control points is determined based on the initial trajectory length (15 control points in this embodiment). Optimization Library: Solve nonlinear least squares problems using the Ceres Solver optimization library; Objective function weights: α=1.0 (rate of change of velocity constraint), β=0.1 (curvature constraint), γ=10.0 (obstacle potential field constraint); Constraint handling: Dynamic constraints, curvature constraints, and collision avoidance constraints are added to the objective function as penalty terms. For example, when the distance between the trajectory and the obstacle is less than d... safe When =5m, add a penalty of 1000×(d) safe - distance)²; Optimize the number of iterations to 80, ensuring a solution is completed within 150ms.

[0067] Step S4: Online rolling execution and local replanning Trajectory tracking: A model predictive controller (MPC) is used to track and optimize the trajectory, with a control period Δt = 1.0s; Perception Update: Environmental information and unmanned surface vessel status are updated once every planning cycle (2.0s); Local replanning parameters: Time window: the next 20 seconds; Spatial window: centered on the current location, with a radius of 80m; Replanning trigger threshold: trajectory tracking deviation > 1.5m, or newly detected obstacle < 5m from the current trajectory; Emergency obstacle avoidance mode: When optimization is not feasible, the objective function is simplified to:

[0068] In emergency scenarios, avoiding collisions is the primary prerequisite: the "time cost" and "energy cost" (such as the velocity change term) in the original objective function are secondary objectives. If they are retained, the optimization algorithm will hesitate between collision avoidance and efficiency, and will be unable to quickly generate a safe trajectory. The penalty characteristic of the potential field function is adapted for emergency obstacle avoidance: when an obstacle approaches, the sharp increase in the potential field value can provide a strong signal for the optimization algorithm to move away, guiding the trajectory to quickly deviate from the danger zone and avoid getting trapped in local optima.

[0069] At this point, the core term 1 is γΦobs(γ(t)), which has the highest collision avoidance priority to ensure survival. Φobs(γ(t)) is the obstacle potential field function. After multiplying by the weight γ (usually a large value, such as γ=10.0 in the original embodiment), this term becomes the dominant term of the objective function. The core objective of optimization becomes minimizing the potential field value, that is, forcing the trajectory away from all obstacles to ensure that the unmanned surface vessel maintains a safe distance from the obstacles.

[0070] Emergency obstacle avoidance should not come at the cost of loss of control: If only the potential field term is retained, the optimization algorithm may generate an extreme sharp turn trajectory (angular velocity exceeding the physical limit of the unmanned surface vessel) in order to get away from the obstacle as quickly as possible, which may cause the unmanned surface vessel to capsize, the actuator to be damaged, or the trajectory to be untrackable. Balancing collision avoidance efficiency with navigation stability: β(ω(t)) 2 As a "smoothing constraint", it can limit the severity of the turning, ensure that the generated trajectory conforms to the dynamic constraints of the unmanned surface vessel (maximum angular velocity ωmax), and avoid attitude loss of control due to sudden changes in heading.

[0071] At this time, β(ω(t)) 2 For core item 2, ensuring stable navigation and avoiding loss of control, ω(t) is the angular velocity (rate of change of heading) of the unmanned surface vessel. 2 This term reflects the severity of course adjustments: the greater the angular velocity (sharp turn), the larger this term value, and the larger the objective function J (penalized); the more stable the angular velocity (slow turn), the smaller this term value, and the smaller the objective function J (encouraged). Multiplied by a weight β (usually a moderate value, such as β=0.1 in the original embodiment), this term constrains the severity of course changes.

[0072] In emergency mode, the velocity change rate term and curvature constraint term are removed. The velocity change rate term aims to minimize velocity fluctuations (balancing energy consumption and stability), but emergency obstacle avoidance may require rapid acceleration to escape or emergency deceleration to avoid an obstacle. Retaining this term limits the flexibility of velocity adjustment, leading to sluggish obstacle avoidance response. For example, when facing an approaching obstacle head-on, the unmanned surface vessel may need to rapidly decelerate and turn. If constrained by the velocity change rate term, it will be unable to adjust its velocity in time, increasing the risk of collision. Curvature κ(t) is strongly correlated with angular velocity ω(t). Retaining the angular velocity term indirectly constrains the stability of curvature. If the angular velocity is stable and there are no extreme fluctuations (the velocity usually does not change abruptly during emergency obstacle avoidance), then the curvature will naturally not exceed the limit. The curvature term involves the second derivative of the position, and its computational complexity is higher than that of the angular velocity term. Removing it can reduce the computational load of the optimization algorithm and ensure that the trajectory is generated within 100-300ms, which meets the real-time requirements of emergency obstacle avoidance.

[0073] The number of iterations is 50, and the solution time is ≤200ms.

[0074] Through simulator testing and actual lake experiments, the method of the present invention demonstrates the following effects in the above-mentioned task scenarios: Safety: The unmanned surface vessel successfully avoided all static islands, shoals and dynamic vessels, with a minimum distance of ≥5m from obstacles, eliminating the risk of collision; Feasibility: The generated optimized trajectory has continuous curvature, with a maximum curvature of 0.15 rad / m (less than the maximum allowable curvature of 0.167 rad / m), smooth velocity changes, no cases exceeding dynamic constraints, and the lower-level controller can accurately track (tracking deviation ≤ 0.8m). High efficiency: The total sailing time is 68 seconds, which is more than 20% higher than the traditional separate method (average sailing time of 85 seconds). Robustness: When a dynamic vessel suddenly changes course (deviation of 10°), the system triggers local replanning within 250ms, generates a new avoidance trajectory, and smoothly connects to the original trajectory without stagnation or oscillation.

[0075] The above results fully verify the effectiveness and superiority of the method of the present invention, and can meet the needs of autonomous navigation and intelligent collision avoidance of unmanned surface vessels in complex and dynamic waters. The above description is merely a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the technical scope disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. An integrated path planning method for unmanned surface vessels based on spatiotemporal state search and optimization, characterized in that, Includes the following steps: Step S1: Construct a unified spatiotemporal state space and dynamics model; Define the extended state of the unmanned surface vessel at time t as: ； Among them, (x t , y t ) represents the planar coordinate position of the unmanned surface vessel at time t, ψ t Represents the heading angle, v t Indicates forward speed; Establish a three-degree-of-freedom discrete kinematic model: ； Where, ω t Let a be the angular velocity of the unmanned surface vessel. t Let Δt be the acceleration of the unmanned surface vessel, and Δt be the control period. Establish control input Its value range is determined by the performance of the unmanned surface vessel's physical actuators, that is... U adm To allow control sets; Step S2: Perform a forward search based on the spatiotemporal state space to generate a preliminary feasible trajectory; use an improved spatiotemporal A* search algorithm or a state lattice search algorithm to search in the state space graph G(S, E), where each node searched is an extended state S. t Each edge E corresponds to a legal transition between two states, which is a short trajectory; Step S3: Perform continuous spatiotemporal optimization on the initial trajectory to generate a smooth, executable trajectory; Step S4: Online rolling execution and local replanning.

2. The integrated path planning method for unmanned surface vessels based on spatiotemporal state search and optimization according to claim 1, characterized in that, Step S2, the search process, begins with the initial position, initial heading, and initial velocity, and iteratively generates a preliminary feasible trajectory through the following sub-steps: Step S21 Node Expansion: For the optimal node St with the lowest cost in the current priority queue, in the permissible control set U... adm The method employs uniform sampling or heuristic sampling to select a set of control inputs u. t For example, consider seven typical action combinations: maintaining speed and heading, accelerating straight ahead, decelerating straight ahead, turning left, turning right, accelerating left, and decelerating right. Substitute each control input into the dynamic model established in step S1 for forward simulation to calculate the subsequent state S at time t+Δt. t+Δt ; Step S22: Spacetime Collision Detection For the generated from S t To S {t+Δt} Collision detection is performed on the state transition edges, including both static obstacle detection and dynamic obstacle detection. Static obstacle detection: First, a static occupancy grid map is constructed, which is generated by fusing pre-loaded high-precision electronic nautical charts with environmental data collected by real-time sensors. Then, it detects whether a state transition edge intersects with an obstacle region in the static occupancy grid map. If they intersect, a collision is identified, and the state transition edge is invalid. Specifically: Based on the static occupancy grid map, the static obstacle set is geometrically reconstructed. The original static obstacle set obtained by fusing electronic charts and sensors is denoted as: ； Each Q i Represented by the set of boundary points, for any two obstacles Q i Q j When the nearest distance is less than the preset merging threshold d merge When these are considered as the same obstacle cluster, convex hull operations are performed on all boundary points within the cluster to obtain the static obstacle convex hull set: ； Based on this, each convex hull is expanded outward according to a safe buffer distance to construct a buffer no-navigation zone: ； This results in a new set of static obstacles: ； During the spatiotemporal search, collision detection is no longer targeted at the original obstacle set O, but is uniformly applied to the buffer no-navigation set. The process proceeds as long as the state transition edge connects to any B. k Intersection is considered a collision; Dynamic obstacle detection: The state of dynamic obstacles is collected in real time through an automatic recognition system, radar, and visual sensors to obtain their position, speed, and heading information. Assuming that the dynamic obstacle is moving in a uniform linear motion within the prediction time window [t, t+Δt], its motion trajectory is extended into a "spacetime tube", which is a tubular region in three-dimensional space (x, y, time) consisting of the obstacle's position, motion range, and time window. The system detects whether the state transition edge intersects with this spacetime tube. If they intersect, it is determined that there is a potential collision risk, and the state transition edge is invalid. Step S23 Cost Evaluation and Heuristic Guidance: For a safe successor node S' that passes collision detection, its total cost needs to be calculated to guide the search algorithm towards the optimal direction; the total cost f(S') consists of two parts: the actual cost and the heuristic cost, expressed as follows: f(S') = g(S') + λ× h(S'); Wherein, λ is a weighting factor used to balance the impact of actual costs and heuristic costs; Actual cost g(S'): represents the cost from the initial state S start The cumulative cost to the current successor node S' is expressed as: g(S')=g(S t )+α×Δt+β×(v t+Δt -v t ) 2 +γ×(ω t ) 2 Where α, β, and γ are weighting coefficients, and Δt is the time cost of the current control cycle; (v t+Δt -v t ) 2 The square of the change in velocity, (ω) t ) 2 The square of the angular velocity; The heuristic cost h(S') consists of the geometric distance term hgeo(S') and the spatiotemporal risk term hrisk(S'), and its expression is: h(S') = hgeo(S') + hrisk(S'); The geometric distance term hgeo(S') represents the estimated geometric distance from the current node S' to the target region, expressed as: ； Where (x', y') are the position coordinates of node S', (x g , y g ) represents the center coordinates of the target region, v curr Let Δt be the current ocean current velocity. curr For the duration of ocean current influence, v max This is the maximum speed of the unmanned surface vessel; h risk (S'): Spatiotemporal risk term, expressed as: ； Where n is the number of dynamic obstacles; TCPA i DCPA represents the shortest predicted collision time between the unmanned surface vessel (USV) and the i-th dynamic obstacle in state S'. i ε is the nearest encounter distance between the unmanned surface vessel and the i-th dynamic obstacle; ε is the risk threshold; δ is the smoothing factor. Step S24 Termination Condition When a state node S that meets the following objective conditions is found... goal Stop the search when: Location conditions: The position coordinates (x, y) of node Sgoal and the center coordinates (x, y) of the target region. g ,y g The distance is less than the set threshold; Velocity condition: Node S goal The sailing speed v is less than the set threshold; After the search stops, from the target node S goal Backtracking to the initial state S start This yields a discrete spatiotemporal state sequence, which is a rough but dynamically feasible and collision-free preliminary trajectory.

3. The integrated path planning method for unmanned surface vessels based on spatiotemporal state search and optimization according to claim 2, characterized in that, Step S3 specifically includes: Step S31: Optimize variable parameterization: The position trajectories x(t) and y(t) are parameterized using cubic uniform B-spline curves, and the expressions are as follows: ； Among them, P x,i P y,i N represents the control points of the B-spline curve. i,3 (t) is a cubic B-spline basis function, k is the number of control points, and the relationship between the heading angle ψ(t) and velocity v is given. (t) Perform B-spline parameterization; Step S32: Constructing the objective function: Minimize the following performance metrics: ； Among them, the first item Used to control the rate of change of path velocity, ensuring that the trajectory accelerates and decelerates smoothly within the limits allowed by the vehicle's dynamics; the second item Used to constrain the curvature of the trajectory, ensuring that the curvature of the generated path is continuous and does not exceed the maximum permissible curvature k. max This satisfies the unmanned surface vessel's heading and turning capabilities; the third item By constraining the trajectory using the potential field of obstacles, the unmanned surface vessel can maintain a safe distance from obstacles. For static obstacles, this invention uses the buffer no-navigation set constructed in step S22 to define the obstacle potential field. For any trajectory point... It reaches the k-th buffer no-fly zone The minimum distance is expressed as ； Then the obstacle potential field function can be constructed: ； in For a single barrier potential function, when When the time approaches zero, The time increases rapidly, thus naturally pushing the trajectory away from the buffer no-navigation zone during the optimization process, achieving consistent modeling of static obstacles in the search and optimization layers; Step S33 Constraints: Dynamic constraints: v min ≤ v(t) ≤ v max |ω(t)| ≤ ω max |a(t)| ≤ a max ; where v min v is the minimum speed of the unmanned surface vessel. max a max ω max The maximum speed, maximum acceleration, and maximum angular velocity of the unmanned surface vessel are determined by hardware performance. Curvature continuity constraint: B-spline parameterization ensures the continuity of the curvature κ(t) and its rate of change of the trajectory, and |κ(t)| ≤ κ. max κ max The maximum permissible curvature is determined by the minimum turning radius of the unmanned surface vessel. R min This achieves the minimum turning radius, fundamentally satisfying the USV minimum turning radius requirement; Collision avoidance constraint: For all obstacles i and time t, distance(γ(t), Obstacle i(t) ) ≥ d safe Obstacle i(t) It is the time-varying position of the dynamic obstacle, d safe It is a safe distance; Step S34: Optimization problem solution: The above optimization objective and constraints are formalized into a nonlinear programming problem, which is then solved using efficient numerical optimization methods such as sequential quadratic programming or interior point method.

4. The integrated path planning method for unmanned surface vessels based on spatiotemporal state search and optimization according to claim 3, characterized in that, Step S4 specifically includes: Step S41, Trajectory Tracking Execution The lower-level controller receives the optimized trajectory generated in step S3, tracks the position, heading and speed of the unmanned surface vessel in real time, and adjusts the control input according to the deviation between the actual state and the trajectory to ensure that the unmanned surface vessel sails accurately according to the optimized trajectory. Step S42, Perception Update: In each planning cycle, the environmental information is updated using the multi-sensor fusion system onboard the unmanned surface vessel, including: Correction of static obstacle maps (e.g., discovery of reefs not marked on electronic charts); Updates on the status of dynamic obstacles (such as changes in the speed and course of other vessels); Accurate estimation of the unmanned surface vessel's own state (such as position, heading, and speed obtained through GPS+IMU fusion); Step S43, Local Replanning Triggering Conditions A new, unexpected obstacle is detected, and this obstacle is located in front of the current execution trajectory, which may lead to a collision; The actual motion state of the dynamic obstacle deviates significantly from the predicted model, and the collision avoidance constraints of the original trajectory are no longer satisfied. The unmanned surface vessel's trajectory tracking deviation exceeds a set threshold due to external interference and cannot be corrected by the lower-level controller. The mission objective has changed (e.g., a new search and rescue point has been added temporarily). Step S44: Local replanning execution Starting from the current actual state of the unmanned surface vessel, repeat steps S2-S3 within a local spatiotemporal window, but limit the search range and time to the current local spatiotemporal window, quickly generate a local trajectory to avoid new risks, and smoothly connect to the global target.

Images