A method and system for unmanned surface vessel path planning based on rule constraints and time synchronization.
By employing a path planning method that combines time synchronization and COLREGs rule constraints, the problem of lack of foresight and rule violations in collision avoidance decisions under dynamic encounter scenarios using the traditional dynamic window method is solved, thus enabling safe and efficient collision avoidance for unmanned vessels.
Patent Information
- Application Number
- CN202611275556.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-08-21
- Publication Date
- 2026-11-17
AI Technical Summary
Traditional dynamic window methods struggle to distinguish the motion characteristics and temporal evolution differences between static obstacles and dynamic target vessels in dynamic encounter scenarios, resulting in collision avoidance decisions that lack foresight and may violate maritime regulations.
By processing shipboard information through time synchronization, coordinate unification, and target association, and combining COLREGs rules to determine encounter scenarios and avoidance responsibilities, candidate control variables are generated. Static and dynamic distance evaluations and rule consistency assessments are then performed to select the optimal navigation control command.
It improves the accuracy of collision avoidance decisions and compliance with maritime rules, reduces the risk of misjudging the encounter situation, and ensures that avoidance actions comply with international maritime collision avoidance rules.
Smart Images

Figure CN123114002A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of intelligent path planning technology, and in particular to a method and system for unmanned surface vessel path planning based on rule constraints and time synchronization. Background Technology
[0002] With the development of intelligent shipping, unmanned surface vessels, and autonomous navigation technologies, autonomous collision avoidance decision-making has become a crucial technical aspect of ensuring maritime navigation safety. Traditional dynamic window methods typically treat shorelines, islands, fixed obstacles, and dynamic target vessels as a single obstacle for distance evaluation, making it difficult to distinguish the differences in motion characteristics, temporal evolution, and collision avoidance responsibilities between static obstacles and dynamic target vessels.
[0003] In dynamic encounter scenarios, the traditional dynamic window method easily overlooks the relationship between the target vessel's future trajectory and the candidate trajectory of the vessel at the same predicted moment, resulting in a lack of foresight in the evaluation of candidate control variables. Furthermore, collision avoidance requires not only physical avoidance of collisions but also compliance with the obligations under the International Regulations for Preventing Collisions at Sea (ICP-5) regarding encounters, crossings, overtaking, yielding, and straight-ahead engagements. If only geometric obstacle avoidance is considered, situations may arise where a collision is avoided, but the avoidance direction or behavior does not comply with maritime regulations, thus increasing the risk of misjudgment by other vessels. Summary of the Invention
[0004] Purpose of the invention: To address the above-mentioned problems, the purpose of this invention is to provide a path planning method and system for unmanned surface vessels based on rule constraints and time synchronization trajectories, applicable to collision avoidance decisions of autonomous vessels, and to achieve autonomous obstacle avoidance in accordance with maritime regulations.
[0005] Technical solution: A path planning method for unmanned surface vessels based on rule constraints and time synchronization, comprising the following steps:
[0006] S1: The ship's status information, target ship status information, static obstacle information and navigation environment information with timestamps are collected in real time by the ship's onboard equipment. The ship's onboard computing unit performs time synchronization, coordinate unification, outlier removal and target association processing on the acquired information to form navigation situation information under a unified spatiotemporal reference.
[0007] S2: Based on the position, heading and speed information of the ship and the target ship, as well as the target's relative bearing angle from the ship's perspective and the target's relative bearing angle from the target's perspective, the two are mapped to the scene determination table based on COLREGs rules to determine the current encounter scene and the ship's avoidance responsibility, and the corresponding rule avoidance command is output.
[0008] S3: Generate candidate control quantities within a dynamic window based on the ship's current speed, angular velocity, and motion constraints, and construct a unified prediction time sequence with the current decision time as the starting time. Generate the ship's candidate predicted trajectory and the target ship's predicted trajectory for each candidate control quantity on the unified prediction time sequence.
[0009] S4: Calculate the minimum static distance between each predicted position and static obstacle in the candidate predicted track of the ship and the candidate predicted track of the ship corresponding to each candidate control quantity, and obtain the static distance evaluation value corresponding to each candidate control quantity.
[0010] S5: Match each candidate position of the ship with the predicted position of the target ship at the same prediction time, calculate the minimum dynamic predicted airspace distance between the candidate trajectory of the ship and the predicted trajectory of the target ship in the prediction time domain, and obtain the dynamic predicted distance evaluation value corresponding to each candidate control quantity.
[0011] S6: Based on the rule-based avoidance command output in step S2, determine the corresponding candidate maneuvering action according to the candidate speed and candidate angular velocity in each candidate control variable, and judge the consistency between the candidate maneuvering action and the rule-based avoidance command to obtain the rule consistency evaluation value corresponding to each candidate control variable.
[0012] S7: Select feasible candidate control quantities based on the static distance evaluation value, dynamic predicted distance evaluation value and rule consistency evaluation value, and conduct a comprehensive evaluation of the feasible candidate control quantities, and select the candidate control quantity with the largest comprehensive evaluation value as the optimal navigation control command;
[0013] S8: The shipborne computing unit sends the optimal navigation control command to the underlying execution control unit, which then executes the optimal navigation control command and feeds back the navigation status during the execution process to the shipborne computing unit and the shore-based monitoring center.
[0014] Furthermore, in step S1:
[0015] The ship's status information includes its position, velocity vector, heading and bow roll angular velocities, and their corresponding acquisition timestamps, which are used to determine the initial motion state of the dynamic window;
[0016] The target vessel status information records the position, velocity vector, heading, and hull dimensions of each target vessel according to the target identifier, as well as the data source identifier and collection timestamp, to form the time-series status information of each target vessel; the static obstacle information includes the navigable water boundary, fixed obstacle boundary, and restricted navigation area boundary, to determine the static feasible range of the candidate trajectory of this vessel;
[0017] The navigation environment information includes water flow speed information, visibility information, positioning error information, and target ship observation error information, which are used to predict trajectory correction or safety distance correction margin;
[0018] In step S1: the current decision time is The most recent observation time of the effective status information of the m-th target ship is According to the m-th target ship at the observation time The position and velocity of the target ship are used to compensate for its position at the current decision time. The compensated target ship state information and the current ship state information are converted to the same coordinate system, and the continuous observation data are correlated with the target based on the target identifier to form the target ship alignment state at the current decision time.
[0019] Furthermore, in step S2, determining the current encounter scenario and the vessel's avoidance responsibility includes the following steps: S21: Let the vessel's position and heading be respectively... The target ship's position and course are respectively The relative bearing of the target ship in its own coordinate system is defined as: S22: Define the relative bearing of this ship in the target ship's coordinate system as:
[0020] ;
[0021] S23: According to the navigation conventions defined in the COLREGs rules, the relative bearing around the ship is divided into four logical sectors, and the relative bearing angle of the target ship in the ship's coordinate system is used. And the relative bearing of this vessel in the target vessel's coordinate system. Mapped to the corresponding logical sectors respectively; according to and The sector mapping results constitute sector combinations ( The system identifies a unique encounter scenario and, based on the identified scenario, outputs the ship's avoidance responsibility and suggested maneuvers.
[0022] Furthermore, in step S3, candidate control quantities are discretely generated within the dynamic window:
[0023] ;
[0024] in, For the i-th group of candidate speeds, Let i be the angular velocity of the i-th candidate group. The number of candidate control variables; based on the current decision time. Using the starting time as the starting time, construct a unified prediction time sequence:
[0025] ;
[0026] in, To predict the number of steps, To predict the step size;
[0027] The candidate predicted trajectory of the ship corresponding to the i-th group of candidate control variables is recursively calculated as follows:
[0028] ;
[0029] ;
[0030] ;
[0031] in, , , These represent the ship's position at the predicted time under the action of the i-th group of candidate control variables. Position and heading.
[0032] Furthermore, in step S3, based on the current decision time as the unified prediction starting point for the m-th target ship, the target ship's position is extrapolated in time synchronization according to the same prediction step size and prediction time sequence as the candidate prediction trajectory of this ship:
[0033] ;
[0034] ;
[0035] in, and Let m be the position of the target ship at the current decision moment after time synchronization processing. To unify the k-th prediction time in the prediction time sequence; the prediction time of each target ship at the prediction time The predicted position and the corresponding ship at the same prediction time for each candidate control quantity. The candidate predicted positions are matched one-to-one.
[0036] Furthermore, in step S4, a static distance evaluation item is set to perform a static navigation safety evaluation on the candidate tracks of the ship corresponding to each candidate control quantity.
[0037] The candidate trajectory of the ship corresponding to the i-th group of candidate control variables is: static obstacle set Then the minimum static safety distance corresponding to the i-th group of candidate control variables is:
[0038] ;
[0039] in, This represents the minimum Euclidean distance from the candidate position of this ship to the set of static obstacles.
[0040] Ideally, in step S5:
[0041] Based on the dimensions of this ship Target ship dimensions Navigation environment information and perceived error Determine the equivalent safe distance between this ship and the m-th target ship: ;
[0042] Dynamic temporal distance evaluation is performed on each candidate control variable in the candidate control variable set, and the dynamic prediction distance evaluation term function is:
[0043] ;
[0044] in, For the i-th group of candidate control variables, the ship at the predicted time... The predicted location, For the m-th target ship at the same predicted time The predicted location.
[0045] Ideally, in step S6, based on the rule-based avoidance instruction output in step S2, a rule consistency evaluation is performed on each candidate control variable in the candidate control variable set. The rule consistency evaluation term function is:
[0046] ;
[0047] in, To avoid the rule instructions, , and These represent right turn, left turn, and keeping direction, respectively. and For candidate speeds and candidate angular velocities, This is the dead zone threshold for angular velocity.
[0048] Ideally, in step S7, a dynamic window method comprehensive evaluation function is constructed based on the static distance evaluation item, the dynamic predicted distance evaluation item, the rule consistency evaluation item, the heading evaluation item, and the speed evaluation item.
[0049] ;
[0050] In the formula, , , and These are the normalized heading evaluation item, static distance evaluation item, dynamic predicted distance evaluation item, and speed evaluation item, respectively. For the rule consistency evaluation item, , , , and These are the corresponding non-negative weighting coefficients;
[0051] The candidate control variable with the largest comprehensive evaluation function value is determined as the optimal control variable at the current planning time. .
[0052] A system employing the above-mentioned rule-constrained and time-synchronized trajectory unmanned vessel path planning method includes a navigation information acquisition and preprocessing module, an encounter scenario and rule determination module, a local path planning module, a low-level execution control module, and a communication and status feedback module.
[0053] The navigation information acquisition and preprocessing module is used to acquire the status information of the ship itself, the target ship, static obstacles, and the navigation environment. It performs time synchronization, coordinate unification, outlier removal, and target association processing on the acquired information to form navigation situation information under a unified spatiotemporal reference.
[0054] The encounter scenario and rule determination module is used to determine the current encounter scenario, the ship's avoidance responsibility, and the rule avoidance instructions based on the position, heading, speed, and bidirectional relative bearing of the ship and the target ship.
[0055] The local path planning module is connected to the navigation information acquisition and preprocessing module and the encounter scenario and rule determination module, respectively. It is used to generate candidate control quantities in the dynamic window, perform time-synchronized prediction of the candidate predicted trajectory of the ship and the predicted trajectory of the target ship, and filter candidate control quantities and determine the optimal navigation control command based on the static distance evaluation value, dynamic predicted distance evaluation value and rule consistency evaluation value.
[0056] The underlying execution control module is connected to the local path planning module and is used to control the propulsion and steering actuators according to the optimal navigation control command.
[0057] The communication and status feedback module is connected to the underlying execution control module and the shipborne computing unit, respectively, to provide feedback on the execution status of the optimal navigation control command and to send the unmanned vessel's navigation status and local path planning results to the shore-based monitoring center.
[0058] Beneficial Effects: Compared with existing technologies, the advantages of this invention are: This invention performs time synchronization, coordinate unification, outlier removal, and target association processing on the ship's status information, target ship status information, static obstacle information, and navigation environment information. This improves the consistency and reliability of multi-source navigation information in time and space for unmanned surface vessels (USVs) local path planning. Identifying encounter scenarios based on bidirectional relative azimuth angles from both the ship's and target ship's perspectives reduces misjudgments of encounter situations and improves the accuracy of identifying typical encounter relationships. Time synchronization evaluation of the ship's candidate trajectory and the target ship's predicted trajectory at the same prediction time, and decomposing the obstacle distance evaluation item into a static distance evaluation item and a dynamic predicted distance evaluation item, improves the relevance and safety of static obstacle avoidance and dynamic target ship avoidance evaluations. Furthermore, a new rule consistency evaluation item improves the maritime rule compliance of collision avoidance decisions. This invention uses discrete candidate control quantities within a dynamic window for prediction, evaluation, and optimization; the calculation process is clear and easy to implement in real-time on a shipboard computing platform. Attached Figure Description
[0059] Figure 1 This is a flowchart illustrating the overall process of the unmanned vessel dynamic window local path planning method of the present invention.
[0060] Figure 2 This is a schematic diagram of the relative orientation sector division of the encounter scene according to the present invention;
[0061] Figure 3 This is a schematic diagram illustrating the time synchronization prediction of the candidate trajectory of the ship and static obstacles in this invention.
[0062] Figure 4 This is a schematic diagram illustrating the time synchronization prediction of the candidate trajectory of the ship and the predicted trajectory of the target ship in this invention.
[0063] Figure 5 This is a simulation diagram of collision avoidance in an encounter scenario according to the present invention;
[0064] Figure 6 This is a simulation diagram of collision avoidance in a cross-encounter scenario according to the present invention;
[0065] Figure 7 This is a simulation diagram of collision avoidance in a chasing scenario according to the present invention. Detailed Implementation
[0066] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that these embodiments are for illustrative purposes only and are not intended to limit the scope of the invention.
[0067] A path planning method for unmanned surface vessels (USVs) based on rule constraints and time-synchronized trajectories is proposed. This method includes navigation information acquisition and preprocessing, encounter scenario and avoidance responsibility determination, dynamic window candidate control variable generation, time-synchronized trajectory prediction, static distance evaluation, dynamic predicted distance evaluation, rule consistency evaluation, comprehensive decision-making, and execution feedback. Figures 1-7 As shown, the specific steps include:
[0068] S1: Shipborne equipment acquires the ship's status information, the target ship's status information, static obstacle information, and navigation environment information. The ship's status information, the target ship's status information, and the navigation environment information are accompanied by corresponding collection timestamps and data source identifiers.
[0069] Specifically, the ship's motion status is obtained through BeiDou / GNSS, inertial navigation IMU, compass, log, or Doppler velocity measurement unit; information on target ships and water surface obstacles is obtained through AIS system, lidar, millimeter-wave radar, visible light camera, or infrared thermal imaging equipment; and navigation environment information is obtained through wind speed and direction instrument, visibility sensor, wave sensor, or hydro-meteorological sensor.
[0070] After receiving the aforementioned multi-source data, the shipborne computing unit performs time synchronization, coordinate unification, outlier removal, and target association processing. Time synchronization unifies data collected by different sensors to the same time reference; coordinate unification converts latitude and longitude coordinates, radar coordinates, visual detection coordinates, and ship coordinates to a local navigation coordinate system with the ship as the reference; outlier removal eliminates abnormal data such as positioning jumps, target tracking loss, low-confidence identification, and data that clearly does not conform to the continuity of target movement; target association matches the same target observation results obtained by AIS, radar, lidar, and visual perception equipment to form unified target ship status information and static obstacle information. After the above processing, the shipborne computing unit forms navigation situation information under a unified spatiotemporal reference.
[0071] S2: Let the ship's position and course be respectively... The target ship's position and course are respectively The relative bearing of the target ship in its own coordinate system is defined as:
[0072] ;
[0073] Define the relative bearing of this ship in the target ship's coordinate system as:
[0074] ;
[0075] To ensure that local collision avoidance decisions meet the requirements of the COLREGs rules and to avoid misjudgments caused by relying solely on one-sided relative bearings, this paper uses two-way relative bearings to determine the encounter situation between two ships. COLREGs categorizes typical ship encounters into three types: overtaking, head-on encounters, and cross encounters. For ease of illustration and subsequent judgment, this paper uses letters to represent the four basic sectors. , , and Representation. Classification methods are as follows: Figure 2 As shown, where, Indicates the sector facing forward of the bow. This indicates the overtaking detection sector behind the stern. Indicates the right-crossing sector. This indicates a left-crossing sector.
[0076] According to the navigation conventions defined in the COLREGs rules, the relative bearing around a ship is divided into four logical sectors, utilizing... The system identifies a unique encounter scenario by comparing the sector combination with a pre-defined scenario determination table, and outputs the ship's avoidance responsibility and suggested maneuvers based on the identified encounter scenario. See Table 1 for details.
[0077]
[0078] S3: Generate a set of candidate control variables within a dynamic window based on the ship's current speed, angular velocity and kinematic constraints, and simultaneously predict the ship's candidate trajectory and the target ship's predicted trajectory within a preset prediction time domain, forming time-aligned predicted trajectories of the two ships.
[0079] The candidate control quantity set U is represented as:
[0080] ;
[0081] in, For the i-th group of candidate speeds, Let i be the angular velocity of the i-th candidate group. The number of candidate control variables; the candidate control variables satisfy the constraints of the ship's maximum speed, minimum speed, maximum angular velocity, minimum angular velocity, and the rate of change of speed and angular velocity.
[0082] S4. Generate candidate control quantities within a dynamic window based on the ship's current speed, angular velocity, and motion constraints. Construct a unified prediction time sequence with the current decision time as the starting time. Generate the ship's candidate predicted trajectory points and the target ship's predicted trajectory points corresponding to each candidate control quantity on the unified prediction time sequence.
[0083] At the current decision-making moment Using the starting time as the starting time, construct a unified prediction time sequence: ;in, To predict the number of steps, The prediction step size is given. The candidate predicted trajectory of the ship corresponding to the i-th group of candidate control variables is recursively calculated as follows:
[0084] ;
[0085] ;
[0086] ;
[0087] in, , , These represent the ship's position at the predicted time under the action of the i-th group of candidate control variables. Position and heading.
[0088] Furthermore, based on the current decision time as the unified prediction starting point for the m-th target ship, and following the same prediction step size and prediction time sequence as the candidate prediction trajectory of this ship, the position of the target ship is extrapolated in time synchronization:
[0089] ;
[0090] ;
[0091] in, and Let m be the position of the target ship at the current decision moment after time synchronization processing. To unify the k-th prediction time in the prediction time sequence; the prediction time of each target ship at the prediction time The predicted position and the corresponding ship at the same prediction time for each candidate control quantity. The candidate predicted positions are matched one-to-one.
[0092] S4: Calculate the minimum static distance between each predicted position and static obstacle in the candidate predicted track of the ship and the candidate predicted track of the ship corresponding to each candidate control quantity, and obtain the static distance evaluation value corresponding to each candidate control quantity.
[0093] The calculation of static distance evaluation items includes the following steps:
[0094] like Figure 3 The candidate predicted trajectory of the ship corresponding to the i-th group of candidate control variables is: The set of static obstacles is defined as the coastline boundary, islands and reefs, fixed obstacles, and non-navigable areas. Then the minimum static safety distance corresponding to the i-th group of candidate control variables is:
[0095] ;
[0096] in, Represents the minimum Euclidean distance from the candidate position of this ship to the set of static obstacles; when When the distance is less than the preset static safety distance threshold, it is determined that the candidate track corresponding to the i-th group of candidate control quantities has a static obstacle collision risk or intrusion risk, and a static obstacle penalty is imposed on the candidate control quantity or it is directly eliminated;
[0097] S5: Match each candidate position of the ship with the predicted position of the target ship at the same prediction time, calculate the minimum dynamic predicted airspace distance between the candidate trajectory of the ship and the predicted trajectory of the target ship in the prediction time domain, and obtain the dynamic predicted distance evaluation value corresponding to each candidate control quantity.
[0098] like Figure 4 For the calculation of the dynamic predicted distance evaluation item, it is based on the ship's dimensions. Target ship dimensions Navigation environment information and perceived error Determine the equivalent safe distance between this ship and the m-th target ship: .
[0099] The minimum dynamic predicted airspace distance of the i-th candidate control variable relative to all target ships in the prediction time domain is:
[0100] ;
[0101] Among them, when If the candidate control variable is determined to have a risk of security path intrusion in the prediction time domain, the control variable will be directly eliminated or subject to a high penalty.
[0102] S6: Based on the rule-based avoidance command output in step S2, determine the corresponding candidate maneuvering action according to the candidate speed and candidate angular velocity in each candidate control variable, and judge the consistency between the candidate maneuvering action and the rule-based avoidance command to obtain the rule consistency evaluation value corresponding to each candidate control variable.
[0103] The rule consistency evaluation item is denoted as The action is determined based on the rule constraint command output in step S2. Let the rule constraint command output in step S2 be... The i-th group of candidate control variables ,in For candidate speeds, Let be the candidate angular velocity, and set This is the dead zone threshold for angular velocity. Then, the consistency evaluation item... for:
[0104] ;
[0105] in, If the value is 1, the steering action corresponding to the candidate control variable is consistent with the current rule constraint action instruction; Setting it to 0 indicates that the steering action corresponding to the candidate control quantity is inconsistent with the current rule constraint action instruction and is suppressed in the comprehensive evaluation.
[0106] S7: Based on the static distance evaluation value, dynamic predicted distance evaluation value, and rule consistency evaluation value, feasible candidate control quantities are screened, and the feasible candidate control quantities are comprehensively evaluated. The candidate control quantity with the largest comprehensive evaluation value is selected as the optimal navigation control command, and the propulsion and steering actuator executes the optimal navigation control command.
[0107] Based on the static distance evaluation item, the dynamic predicted distance evaluation item, the rule consistency evaluation item, the heading evaluation item, and the speed evaluation item, a dynamic window method comprehensive evaluation function is constructed:
[0108] ;
[0109] In the formula, , , and These are the normalized heading evaluation item, static distance evaluation item, dynamic predicted distance evaluation item, and speed evaluation item, respectively. For the rule consistency evaluation item, , , , and represents the corresponding non-negative weighting coefficient.
[0110] The candidate control variable with the largest comprehensive evaluation function value is determined as the optimal control variable at the current planning time. .
[0111] S8: The shipborne computing unit sends the optimal navigation control command to the underlying execution control unit, which then executes the optimal navigation control command and feeds back the navigation status during the execution process to the shipborne computing unit and the shore-based monitoring center.
[0112] 1. Scenario Application and Experimental Effect Analysis:
[0113] Simulation Experiments and Result Analysis: To verify the effectiveness and superiority of the "Unmanned Vessel Dynamic Window Local Path Planning System Based on Rule Constraints and Time Synchronization Trajectory Prediction" described in this invention, three typical encounter scenarios (head-on encounter, cross encounter, and overtaking) conforming to the International Maritime Collision Avoidance Regulations (COLREGs) were set up in the simulation environment. Scenario 1: Head-on Scenario Verification Analysis as follows: Figure 5 As shown, the OS and the TS are initially sailing towards each other, forming a typical encounter situation.
[0114] Scenario 2: Crossing Scenario - Verification Analysis as follows Figure 6 As shown, when the ship (OS) was sailing north, the target ship (TS) cut in from the bow of the ship's starboard side, creating a cross-ship encounter on the starboard side.
[0115] Scenario 3: Overtaking Scenario Verification Analysis Figure 7 As shown, the speed of the ship (OS) is greater than that of the target ship (TS) directly in front of it. The ship gradually approaches the target ship from behind, thus creating a situation of overtaking.
[0116] In summary, simulation experiments demonstrate that this invention is based on the ability to accurately map COLREGs rules under realistic and urgent situations. The improved DWA can plan excellent trajectories that are physically collision-free, legally compliant, and consistent with the driving habits of human navigators.
[0117] The system employing the above-mentioned path planning method for unmanned surface vessels based on rule constraints and time synchronization specifically includes: a navigation information acquisition and preprocessing module, which is used to acquire the vessel's status information, the target vessel's status information, static obstacle information, and navigation environment information, and to perform time synchronization, coordinate unification, outlier removal, and target association processing on the acquired information to form navigation situation information under a unified spatiotemporal reference;
[0118] The encounter scenario and rule determination module is used to determine the current encounter scenario, the ship's avoidance responsibility, and the rule-based avoidance instructions based on the position, heading, speed, and bidirectional relative bearing of the ship and the target ship.
[0119] The local path planning module is connected to the navigation information acquisition and preprocessing module and the encounter scenario and rule determination module, respectively. It is used to generate candidate control quantities in the dynamic window, perform time-synchronized prediction of the candidate predicted trajectory of the ship and the predicted trajectory of the target ship, and filter candidate control quantities and determine the optimal navigation control command based on the static distance evaluation value, the dynamic predicted distance evaluation value and the rule consistency evaluation value.
[0120] The underlying execution control module, connected to the local path planning module, is used to control the propulsion and steering actuators according to the optimal navigation control command;
[0121] The communication and status feedback module is connected to the underlying execution control module and the shipborne computing unit, respectively, and is used to provide feedback on the execution status of the optimal navigation control command and send the unmanned vessel's navigation status and local path planning results to the shore-based monitoring center.
Claims
1. A path planning method for unmanned surface vessels based on rule constraints and time synchronization trajectories, characterized in that... Includes the following steps: S1: The ship's status information, target ship status information, static obstacle information and navigation environment information with timestamps are collected in real time by the ship's onboard equipment. The ship's onboard computing unit performs time synchronization, coordinate unification, outlier removal and target association processing on the acquired information to form navigation situation information under a unified spatiotemporal reference. S2: Based on the position, heading and speed information of the ship and the target ship, as well as the target's relative bearing angle from the ship's perspective and the target's relative bearing angle from the target's perspective, the two are mapped to the scene determination table based on COLREGs rules to determine the current encounter scene and the ship's avoidance responsibility, and the corresponding rule avoidance command is output. S3: Generate candidate control quantities within a dynamic window based on the ship's current speed, angular velocity, and motion constraints, and construct a unified prediction time sequence with the current decision time as the starting time. Generate the ship's candidate predicted trajectory and the target ship's predicted trajectory for each candidate control quantity on the unified prediction time sequence. S4: Calculate the minimum static distance between each predicted position and static obstacle in the candidate predicted track of the ship and the candidate predicted track of the ship corresponding to each candidate control quantity, and obtain the static distance evaluation value corresponding to each candidate control quantity. S5: Match each candidate position of the ship with the predicted position of the target ship at the same prediction time, calculate the minimum dynamic predicted airspace distance between the candidate trajectory of the ship and the predicted trajectory of the target ship in the prediction time domain, and obtain the dynamic predicted distance evaluation value corresponding to each candidate control quantity. S6: Based on the rule-based avoidance command output in step S2, determine the corresponding candidate maneuvering action according to the candidate speed and candidate angular velocity in each candidate control variable, and judge the consistency between the candidate maneuvering action and the rule-based avoidance command to obtain the rule consistency evaluation value corresponding to each candidate control variable. S7: Select feasible candidate control quantities based on the static distance evaluation value, dynamic predicted distance evaluation value and rule consistency evaluation value, and conduct a comprehensive evaluation of the feasible candidate control quantities, and select the candidate control quantity with the largest comprehensive evaluation value as the optimal navigation control command; S8: The shipborne computing unit sends the optimal navigation control command to the underlying execution control unit, which then executes the optimal navigation control command and feeds back the navigation status during the execution process to the shipborne computing unit and the shore-based monitoring center.
2. The path planning method for unmanned surface vessels based on rule constraints and time synchronization as described in claim 1, characterized in that, In step S1: The ship's status information includes its position, velocity vector, heading and bow roll angular velocities, and their corresponding acquisition timestamps, which are used to determine the initial motion state of the dynamic window; The target vessel status information records the position, velocity vector, heading, and hull dimensions of each target vessel according to the target identifier, as well as the data source identifier and collection timestamp, to form the time-series status information of each target vessel; the static obstacle information includes the navigable water boundary, fixed obstacle boundary, and restricted navigation area boundary, to determine the static feasible range of the candidate trajectory of this vessel; The navigation environment information includes water flow speed information, visibility information, positioning error information, and target ship observation error information, which are used to predict trajectory correction or safety distance correction margin; In step S1: the current decision time is The most recent observation time of the effective status information of the m-th target ship is According to the m-th target ship at the observation time The position and velocity of the target ship are used to compensate for its position at the current decision time. The compensated target ship state information and the current ship state information are converted to the same coordinate system, and the continuous observation data are correlated with the target based on the target identifier to form the target ship alignment state at the current decision time.
3. The path planning method for unmanned surface vessels based on rule constraints and time synchronization as described in claim 1, characterized in that, In step S2, the current encounter scenario and the vessel's avoidance responsibility are determined. The following steps are included: S21: Let the ship's position and heading be respectively... The target ship's position and course are as follows: The relative bearing of the target ship in its own coordinate system is defined as: S22: Define the relative bearing of this ship in the target ship's coordinate system as: ; S23: According to the navigation conventions defined in the COLREGs rules, the relative bearing around the ship is divided into four logical sectors, and the relative bearing angle of the target ship in the ship's coordinate system is used. And the relative bearing of this vessel in the target vessel's coordinate system. Mapped to the corresponding logical sectors respectively; according to and The sector mapping results constitute sector combinations ( The system identifies a unique encounter scenario and, based on the identified scenario, outputs the ship's avoidance responsibility and suggested maneuvers.
4. The path planning method for unmanned surface vessels based on rule constraints and time synchronization as described in claim 1, characterized in that, In step S3, candidate control quantities are generated discretely within the dynamic window: ; in, For the i-th group of candidate speeds, Let i be the angular velocity of the i-th candidate group. The number of candidate control variables; based on the current decision time. Using the starting time as the starting time, construct a unified prediction time sequence: ; in, To predict the number of steps, To predict the step size; The candidate predicted trajectory of the ship corresponding to the i-th group of candidate control variables is recursively calculated as follows: ; ; ; in, , , These represent the ship's position at the predicted time under the action of the i-th group of candidate control variables. Position and heading.
5. The path planning method for unmanned surface vessels based on rule constraints and time synchronization as described in claim 1, characterized in that, In step S3, based on the current decision time as the unified prediction starting point for the m-th target ship, the target ship's position is extrapolated in time synchronization according to the same prediction step size and prediction time sequence as the candidate prediction trajectory of this ship: ; ; in, and Let m be the position of the target ship at the current decision moment after time synchronization processing. To unify the k-th prediction time in the prediction time sequence; the prediction time of each target ship at the prediction time The predicted position and the corresponding ship at the same prediction time for each candidate control quantity. The candidate predicted positions are matched one-to-one.
6. The path planning method for unmanned surface vessels based on rule constraints and time synchronization as described in claim 1, characterized in that, In step S4, a static distance evaluation item is set to perform a static navigation safety evaluation on the candidate tracks of the ship corresponding to each candidate control quantity. The candidate trajectory of the ship corresponding to the i-th group of candidate control variables is: static obstacle set Then the minimum static safety distance corresponding to the i-th group of candidate control variables is: ; in, This represents the minimum Euclidean distance from the candidate position of this ship to the set of static obstacles.
7. The path planning method for unmanned surface vessels based on rule constraints and time synchronization as described in claim 1, characterized in that, In step S5: Based on the dimensions of this ship Target ship dimensions Navigation environment information and perceived error Determine the equivalent safe distance between this ship and the m-th target ship: ; Dynamic temporal distance evaluation is performed on each candidate control variable in the candidate control variable set, and the dynamic prediction distance evaluation term function is: ; in, For the i-th group of candidate control variables, the ship at the predicted time... The predicted location, For the m-th target ship at the same predicted time The predicted location.
8. The path planning method for unmanned surface vessels based on rule constraints and time synchronization as described in claim 1, characterized in that, In step S6, based on the rule-based avoidance instruction output in step S2, a rule consistency evaluation is performed on each candidate control variable in the candidate control variable set. The rule consistency evaluation term function is: ; in, To avoid the rule, , and These represent right turn, left turn, and keeping direction, respectively. and For candidate speeds and candidate angular velocities, This is the dead zone threshold for angular velocity.
9. The path planning method for unmanned surface vessels based on rule constraints and time synchronization as described in claim 1, characterized in that, In step S7, a dynamic window method comprehensive evaluation function is constructed based on the static distance evaluation item, the dynamic predicted distance evaluation item, the rule consistency evaluation item, the heading evaluation item, and the speed evaluation item: ; In the formula, , , and These are the normalized heading evaluation item, static distance evaluation item, dynamic predicted distance evaluation item, and speed evaluation item, respectively. For the rule consistency evaluation item, , , , and These are the corresponding non-negative weighting coefficients; The candidate control variable with the largest comprehensive evaluation function value is determined as the optimal control variable at the current planning time. .
10. A system employing the path planning method for unmanned surface vessels based on rule constraints and time synchronization as described in any one of claims 1 to 9, characterized in that: It includes a navigation information acquisition and preprocessing module, an encounter scenario and rule determination module, a local path planning module, a low-level execution control module, and a communication and status feedback module; The navigation information acquisition and preprocessing module is used to acquire the status information of the ship itself, the target ship, static obstacles, and the navigation environment. It performs time synchronization, coordinate unification, outlier removal, and target association processing on the acquired information to form navigation situation information under a unified spatiotemporal reference. The encounter scenario and rule determination module is used to determine the current encounter scenario, the ship's avoidance responsibility, and the rule avoidance instructions based on the position, heading, speed, and bidirectional relative bearing of the ship and the target ship. The local path planning module is connected to the navigation information acquisition and preprocessing module and the encounter scenario and rule determination module, respectively. It is used to generate candidate control quantities in the dynamic window, perform time-synchronized prediction of the candidate predicted trajectory of the ship and the predicted trajectory of the target ship, and filter candidate control quantities and determine the optimal navigation control command based on the static distance evaluation value, dynamic predicted distance evaluation value and rule consistency evaluation value. The underlying execution control module is connected to the local path planning module and is used to control the propulsion and steering actuators according to the optimal navigation control command. The communication and status feedback module is connected to the underlying execution control module and the shipborne computing unit, respectively, to provide feedback on the execution status of the optimal navigation control command and to send the unmanned vessel's navigation status and local path planning results to the shore-based monitoring center.
