ROS2-based tugboat intelligent auxiliary driving system data and interaction method

Abstract

The present application relates to the technical field of water traffic control, and particularly relates to a tugboat intelligent auxiliary driving system data and interaction method based on ROS2. The tugboat intelligent auxiliary driving system data and interaction method based on ROS2 comprises the following steps: S1: taking a ship as a mother ship and a tugboat as a child ship, obtaining mother ship data, child ship data and external environment data; obtaining mother ship running data and child ship running data based on the mother ship data and the child ship data, and obtaining deviation consistency between the child ship and the mother ship according to the mother ship running data and the child ship running data. The present application is based on ROS2, fuses mother ship, child ship and environment data, constructs a deviation consistency and system reliability weight model, combines a distributed model predictive control and a dynamic obstacle avoidance strategy, realizes cooperative control and global optimization of a tugboat formation, and improves the safety, efficiency and intelligent level of water traffic.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of water traffic control, and in particular to a tugboat intelligent auxiliary driving system data and interaction method based on ROS2. BACKGROUND

[0002] With the growth of port throughput and the trend of large-scale ships, the traffic density of the port and the adjacent water area is increasing, and the intelligent and collaborative level of the water traffic control system is put forward with higher requirements. As the core force of assisting large ships to enter and exit the port, the operation efficiency and safety of the tugboat are directly related to the smoothness and safety of the entire port traffic flow.

[0003] At present, the existing water traffic management system relies on the manual scheduling of the VTS (Vessel Traffic Service) center and the autonomous collision avoidance of the ship, and has the following limitations: (1) Lack of multi-target collaborative control capability: it is difficult to carry out unified and collaborative path planning and dynamic scheduling for the operation formation composed of the mother ship and multiple tugboats; (2) Obstacle avoidance decision isolation: the collision avoidance decision of each ship is usually based on its own perception information, lacking global optimization at the system level, which is easy to cause conflict or suboptimal avoidance action; (3) Lack of system reliability evaluation: the existing system cannot quantitatively evaluate the influence of communication delay, environmental disturbance and equipment state on the overall control reliability, and it is difficult to adaptively adjust the control strategy in complex environment, thereby causing traffic congestion and even safety accidents.

[0004] Therefore, an intelligent water traffic control method capable of realizing tugboat cluster collaborative control, having global optimization obstacle avoidance decision capability and being able to dynamically evaluate system reliability is urgently needed to improve the traffic efficiency and safety level of the port water area. SUMMARY

[0005] In order to overcome the shortcomings of local decision and global control contradiction, the present application provides a tugboat intelligent auxiliary driving system data and interaction method based on ROS2.

[0006] The technical implementation scheme of the present application is: a tugboat intelligent auxiliary driving system data and interaction method based on ROS2, comprising the following steps:

[0007] S1: taking a ship as a mother ship and a tugboat as a child ship, obtaining mother ship data, child ship data and external environment data; obtaining mother ship running data and child ship running data based on the mother ship data and the child ship data, and obtaining deviation consistency between the child ship and the mother ship according to the mother ship running data and the child ship running data;

[0008] S2: obtaining the mother ship maneuvering data and the sub ship maneuvering data based on the mother ship data and the sub ship data, and generating a control error time sequence according to the mother ship maneuvering data and the sub ship maneuvering data; determining a historical abnormal environment stress sub-period according to the control error time sequence;

[0009] S3: generating a historical abnormal control error amplitude sequence according to the historical abnormal environment stress sub-period, and determining a control fusion period according to the historical abnormal control error amplitude sequence; jointly analyzing the deviation consistency and the control fusion period, and quantifying as a system reliability weight factor;

[0010] S4: generating a conservative obstacle avoidance path and an aggressive obstacle avoidance path according to the system reliability weight factor; obtaining a cooperative obstacle avoidance path set according to the conservative obstacle avoidance path and the aggressive obstacle avoidance path, and converting the cooperative obstacle avoidance path set into corresponding thrust and rudder angle control instructions.

[0011] Preferably, the ship is taken as the mother ship, and the tugboat is taken as the sub ship, and the mother ship data, the sub ship data and the external environment data are obtained, including:

[0012] The mother ship data is divided into mother ship running data and mother ship maneuvering data;

[0013] The sub ship data is divided into sub ship running data and sub ship maneuvering data;

[0014] The external environment data is divided into historical environment data and real-time environment data.

[0015] Preferably, the mother ship running data and the sub ship running data are obtained based on the mother ship data and the sub ship data, and the deviation consistency between the sub ship and the mother ship is obtained according to the mother ship running data and the sub ship running data, including:

[0016] The mother ship state data and the sub ship state data are obtained based on the mother ship running data and the sub ship running data;

[0017] The mother ship state data and the sub ship state data include position data, heading data and speed data;

[0018] The mother ship state time sequence and the sub ship state time sequence are constructed according to the time-aligned mother ship state data and the time-aligned sub ship state data;

[0019] The mother ship deviation amount time sequence is generated by calculating the mother ship state change amount at adjacent time according to the mother ship state time sequence;

[0020] The sub ship deviation amount time sequence is generated by calculating the sub ship state change amount at adjacent time according to the sub ship state time sequence;

[0021] The mother ship deviation amount time sequence and the sub-ship deviation amount time sequence are time-delay aligned, a Pearson correlation coefficient between the two sequences is calculated, and the Pearson correlation coefficient is defined as a deviation consistency between the sub-ship and the mother ship.

[0022] Preferably, the mother ship data and the sub-ship data are used to obtain mother ship control data and sub-ship control data, and a control error time sequence is generated according to the mother ship control data and the sub-ship control data, including:

[0023] Based on the mother ship control data and the sub-ship control data, time-aligned mother ship control instructions and sub-ship control feedback are obtained.

[0024] The mother ship control instructions include thrust instructions and rudder angle instructions, and the sub-ship control feedback includes thrust feedback and rudder angle feedback.

[0025] According to the time-aligned mother ship control instructions and sub-ship control feedback, an instruction tracking error at the same time is calculated, and a control error time sequence is generated.

[0026] The instruction tracking error is the difference between the sub-ship control feedback and the mother ship control instructions.

[0027] Preferably, the historical abnormal environment stress sub-period is determined according to the control error time sequence, including:

[0028] Based on the control error time sequence, data points with a persistence higher than a preset persistence threshold and data points with an intermittence higher than a preset intermittence threshold are extracted to form a historical joint abnormal data set.

[0029] The historical joint abnormal data set is used to mark specific time points at which system abnormalities occur in history.

[0030] Based on the historical environment data at the time points corresponding to the historical joint abnormal data set, a continuous period in which the environmental stress exceeds a preset environmental threshold is extracted to form a historical abnormal environment stress period set.

[0031] The historical abnormal environment stress period set is divided into M historical abnormal environment stress sub-periods according to a preset time granularity.

[0032] Preferably, the historical abnormal control error amplitude sequence is generated according to the historical abnormal environment stress sub-period, and a control fusion period is determined according to the historical abnormal control error amplitude sequence, including:

[0033] Based on the historical control error data corresponding to the historical joint anomaly dataset, the control error time series sequence aligned with the time of the M historical anomaly environmental stress sub-cycles is extracted, and the average amplitude of the control error in each sub-cycle is calculated to generate a historical anomaly control error amplitude sequence.

[0034] The duration of each historical abnormal environmental stress sub-cycle is calculated, and the Pearson correlation coefficient between it and the average amplitude of the corresponding historical abnormal control error amplitude sequence is calculated. The Pearson correlation coefficient is defined as the environmental-error correlation strength coefficient.

[0035] The control fusion period is determined based on the calculated environment-error correlation strength coefficient. The rule for determining the control fusion period is: the duration of the historical abnormal environmental stress sub-cycle corresponding to the absolute value of the environment-error correlation strength coefficient exceeding the preset correlation threshold is determined as the control fusion period.

[0036] Preferably, the step of jointly analyzing the deviation consistency and the control fusion period to quantify it into a system reliability weighting factor includes the following joint analysis formula: ,in, As a system reliability weighting factor, For consistency of deviation, To control the fusion cycle, and This is a preset weighted index used to adjust the contribution of the two indicators.

[0037] Preferably, the step of generating conservative obstacle avoidance paths and proactive obstacle avoidance paths based on the system reliability weighting factor includes:

[0038] The system reliability weighting factor is used as an input parameter and input into the local path planning algorithm of the sub-ship; the local path planning algorithm adopts the dynamic window method.

[0039] The system reliability weighting factor is used to dynamically adjust the cost function weight and sampling window size in the path planning algorithm;

[0040] The specific rules for dynamic adjustment are as follows: if the system reliability weight factor is low, the weight of obstacle distance cost is increased, and the range of the velocity sampling window is narrowed to generate a conservative obstacle avoidance path.

[0041] If the system reliability weight factor is high, the weights of path smoothness and schedule cost are increased, and the range of the speed sampling window is expanded to generate an active obstacle avoidance path.

[0042] Preferably, the step of obtaining a cooperative obstacle avoidance path set based on the conservative obstacle avoidance path and the active obstacle avoidance path, and converting the cooperative obstacle avoidance path set into corresponding thrust and rudder angle control commands, includes:

[0043] The path deviation of the mother ship is calculated based on the expected trajectory of the mother ship and the real-time status data in the mother ship's operation data.

[0044] The degree of path deviation is quantified by calculating the Hausdorff distance between the actual track point sequence of the mother ship and the expected track point sequence;

[0045] A multi-objective optimization function is constructed with the goal of minimizing the path deviation of the mother ship as the global optimization objective and the obstacle avoidance paths generated by each of the sub-ships according to the system reliability weight factor as the constraints.

[0046] The multi-objective optimization function simultaneously considers the collision avoidance constraints and kinematic constraints between each sub-ship;

[0047] The multi-objective optimization function is solved using a distributed model predictive control algorithm to generate the final set of cooperative obstacle avoidance paths for each sub-ship.

[0048] The distributed model predictive control algorithm employs a rolling time-domain optimization strategy, resolving the optimization problem based on the latest system state in each control cycle;

[0049] The final set of cooperative obstacle avoidance paths for each sub-ship is converted into corresponding thrust and rudder angle control commands.

[0050] Preferably, the step of converting the final set of cooperative obstacle avoidance paths for each sub-ship into corresponding thrust and rudder angle control commands includes:

[0051] The command is sent to each of the sub-ships for execution via the ROS2 Action communication mechanism.

[0052] The control commands include longitudinal thrust commands and lateral rudder angle commands required for path tracking.

[0053] Beneficial Effects: This invention enhances the collaborative control capability of water traffic: By constructing a deviation consistency index and a distributed model predictive control (DMPC) algorithm, unified collaborative control of the mother ship-tugboat formation is achieved, avoiding the problems of isolated decision-making and conflicting actions of individual units in the traditional mode, and ensuring the orderliness and efficiency of port traffic flow. This invention enhances the dynamic obstacle avoidance and global optimization capability in complex waters: By introducing a system reliability weight factor and adaptively generating conservative or proactive obstacle avoidance paths, the system can dynamically optimize the overall passage path of the formation while ensuring safety, effectively reducing route congestion and traffic delays caused by obstacle avoidance behavior. This invention improves the reliability and robustness of the traffic control system: By fusing multi-source data and analyzing historical anomaly patterns, the system reliability weight is quantified, enabling the control strategy to adaptively adjust according to communication status and environmental disturbance factors, significantly improving the stability and fault tolerance of the traffic control system under complex disturbances. This invention provides core algorithm support for the Intelligent Water Traffic Management System (ITS): This invention is implemented based on the ROS2 framework, with good modularity, scalability, and integration. As the core decision-making module of the intelligent water traffic control system, it promotes the development of water traffic management towards automation and intelligence. Attached Figure Description

[0054] Fig. 1 This is a flowchart of the data and interaction method of the intelligent assisted driving system for tugboats based on ROS2 according to the present invention;

[0055] Fig. 2 This is a flowchart of the deviation consistency acquisition method of the present invention. Detailed Implementation

[0056] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0057] Data and interaction methods for a tugboat intelligent assisted driving system based on ROS2, such as Figs. 1-2 As shown, it includes the following steps:

[0058] It should be noted that in the ROS2-based intelligent assisted driving system for tugboats described in this invention, the unified collection, parsing, forwarding, and monitoring of data from lower-level hardware components are achieved by relying on the ROS2 distributed architecture and its communication mechanisms (including publish-subscribe, service requests, and actions). Data collection is conducted through a multi-node parallel approach, collecting raw message data according to the communication protocols of each hardware component (such as TCP and Modbus). Data parsing involves each node converting data into unified formatted status data (such as position, heading, and speed) and control data (such as thrust and rudder angle) according to a preset protocol. Data forwarding is achieved through ROS2 topic broadcasting, with upper-level algorithm modules subscribing on demand. Data monitoring continuously records the status of all data links, supporting anomaly investigation and post-event analysis. The system runs on a high-performance workstation equipped with Ubuntu 22.04 and the ROS2 environment, exhibiting excellent real-time performance and reliability, providing a multi-source, heterogeneous, and time-consistent data foundation for subsequent deviation consistency calculations, system reliability assessments, and collaborative control.

[0059] S1-1: Using a ship as the mother ship and a tugboat as the daughter ship, acquire data from the mother ship, daughter ship, and external environment.

[0060] The mothership data is divided into mothership operation data and mothership control data;

[0061] The sub-ship data is divided into sub-ship operation data and sub-ship control data;

[0062] The external environment data is divided into historical environment data and real-time environment data.

[0063] It should be noted that in the ROS2-based intelligent assisted driving system for tugboats, data acquisition for both the mother ship and the tugboats relies on the ROS2 distributed architecture. The mother ship and tugboats collect operational data (such as position, heading, and speed) and control data (such as thrust commands and rudder angle feedback) through their onboard sensors and control systems. External environmental data is obtained through meteorological sensors, AIS, and radar equipment, acquiring historical and real-time information. This data undergoes multi-protocol parsing (such as TCP and Modbus) and unified formatting via ROS2 nodes, achieving efficient aggregation and transmission through a publish-subscribe mechanism. The data acquisition process combines real-time performance with reliability, providing a multi-source heterogeneous data foundation for subsequent calculations of deviation consistency, system status assessment, and collaborative obstacle avoidance.

[0064] S1-2: Based on the mother ship data and the daughter ship data, obtain the mother ship operation data and the daughter ship operation data, and obtain the deviation consistency between the daughter ship and the mother ship according to the mother ship operation data and the daughter ship operation data;

[0065] The mother ship status data and the daughter ship status data are based on the acquisition time alignment of the mother ship operation data and the daughter ship operation data;

[0066] The mother ship status data and the daughter ship status data include position data, heading data and speed data;

[0067] Based on the time-aligned mother ship status data and daughter ship status data, construct a mother ship status time sequence and a daughter ship status time sequence.

[0068] The changes in the mother ship's state at adjacent times are calculated based on the mother ship's state time series, and a mother ship deviation time series is generated.

[0069] The changes in the state of the sub-ship at adjacent times are calculated based on the sub-ship state time series to generate the sub-ship deviation time series.

[0070] The time series sequence of the mother ship deviation and the time series sequence of the daughter ship deviation are subjected to time delay alignment processing, the Pearson correlation coefficient between the two sequences is calculated, and the Pearson correlation coefficient is defined as the deviation consistency between the daughter ship and the mother ship.

[0071] It should be noted that the reference Fig. 2 As shown, in the ROS2-based intelligent assisted driving system for tugboats, deviation consistency calculation is the core of achieving collaborative control. Due to communication and response delays between the mother ship and the hull ship, time alignment processing of position, heading, and speed status data is required to ensure data comparability. This status data comprehensively reflects the ship's motion characteristics: position determines spatial relationships, heading indicates the direction of motion, and speed affects dynamic response.

[0072] Separate state time series sequences for the mother ship and the daughter ship are constructed. A deviation time series sequence is generated by calculating the state changes between adjacent time points. The mother ship deviation time series sequence reflects the mother ship's own motion change trend, while the daughter ship deviation time series sequence reflects the daughter ship's actual execution of control commands. Considering the inherent delay in command transmission and execution, delay alignment processing is required for the two deviation sequences.

[0073] Deviation consistency is quantified by calculating the Pearson correlation coefficient between two aligned sequences: ,in, For consistency of deviation, and These represent the deviations of the mother ship and the daughter ship, respectively. and For the corresponding mean, This represents the total number of valid data points within the time-series data window used to calculate the correlation. The Pearson correlation coefficient objectively reflects the accuracy with which the daughter ship follows the mother ship's motion commands, providing crucial decision-making support for subsequent coordinated control.

[0074] S2-1: Obtain mother ship control data and daughter ship control data based on the mother ship data and the daughter ship data, and generate a control error timing sequence based on the mother ship control data and the daughter ship control data;

[0075] Based on the mother ship control data and the daughter ship control data, obtain time-aligned mother ship control commands and daughter ship control feedback.

[0076] The mother ship control commands include thrust commands and rudder angle commands, and the daughter ship control feedback includes thrust feedback and rudder angle feedback.

[0077] Based on the time-aligned mother ship control commands and daughter ship control feedback, calculate the command tracking error at the same moment and generate a control error timing sequence.

[0078] The command tracking error is the difference between the sub-ship control feedback and the mother ship control command.

[0079] It's important to note that in ROS2-based intelligent assisted driving systems for tugboats, the processing of control data is a crucial step in achieving precise control. Unlike operational data, which reflects the ship's motion state, control data directly reflects control commands and their execution effects, including thrust commands, thrust feedback, rudder angle commands, and rudder angle feedback. This data undergoes time-alignment processing through the ROS2 architecture to ensure the consistency of command and feedback timing.

[0080] The specific steps for generating the control error timing sequence are as follows: First, obtain the time-aligned mother ship control commands and daughter ship control feedback, and then calculate the command tracking error at the same moment: , ,in, Let be the thrust feedback value at time t. Let t be the thrust command value. Let t be the rudder angle feedback value. The rudder angle command value at time t. and These represent the thrust error and rudder angle error at time t, respectively. Arranging these error values ​​in chronological order constitutes the control error time series.

[0081] For example, if the mother ship issues a 1000N thrust command, but the stern ship actually generates 980N thrust, then the thrust error at that moment is -20N. This error sequence accurately reflects the stern ship's tracking accuracy of the control command, providing an important basis for subsequent abnormal operating condition identification and system reliability assessment. By continuously monitoring these error data, the performance degradation of the actuators or the impact of environmental interference can be detected in a timely manner.

[0082] S2-2: Determine the historical abnormal environmental stress sub-periods based on the control error time sequence;

[0083] Based on the control error time series, data points that are persistently higher than a preset persistence threshold and data points that are intermittently higher than a preset intermittent threshold are extracted to form a historical joint anomaly dataset;

[0084] The historical joint anomaly dataset is used to mark the specific time points in history when system anomalies occurred;

[0085] Based on the historical environmental data at the corresponding time points of the historical joint anomaly dataset, the duration of environmental stress exceeding a preset environmental threshold is extracted to form a historical anomaly environmental stress time period set.

[0086] The historical abnormal environmental stress time period set is divided into M historical abnormal environmental stress sub-cycles according to a preset time granularity.

[0087] It should be noted that in the ROS2-based intelligent assisted driving system for tugboats, the identification of historical abnormal environmental stress sub-cycles is a crucial step in assessing system reliability. First, based on the control error time series, abnormal data points are extracted according to preset persistence thresholds and preset intermittent thresholds: data points with persistence exceeding the preset persistence threshold (e.g., thrust error continuously exceeding 50N or rudder angle error exceeding 5 degrees), and data points with instantaneous values ​​exceeding a higher preset intermittent threshold (e.g., thrust error instantaneously exceeding 100N or rudder angle error instantaneously exceeding 10 degrees) are marked, collectively forming a historical joint abnormal dataset. Persistent anomalies typically reflect problems within the control system itself, while intermittent anomalies are often caused by data transmission packet loss or transient interference.

[0088] By analyzing historical environmental data corresponding to these anomalous data points, the duration of environmental stress (such as wind speed exceeding 10 m / s or wave height exceeding 2 meters, exceeding safe operating thresholds) is extracted. These durations reflect the substantial impact of the external environment on ship control. The anomalous environmental stress periods are divided into M sub-cycles at a preset time granularity (e.g., 15 minutes) to facilitate the quantitative analysis of the correlation between environmental stress and control error. For example, a continuous one-hour period of high winds and waves is divided into four 15-minute sub-cycles for subsequent calculation of the correlation between environmental stress and control error within each sub-cycle. This segmented processing method can accurately identify the impact patterns of environmental stress on the control system, providing data support for system reliability assessment.

[0089] Preset persistence threshold ( The setting method for this threshold is as follows: This threshold is used to detect long-term anomalies caused by system performance degradation or persistent strong interference. Its setting should be based on the statistical characteristics of historical error data of the system under long-term normal operating conditions. Specifically, the method is to collect normal operating condition data for a sufficient period (e.g., several months) and calculate the mean values ​​of thrust error and rudder angle error respectively. ) and standard deviation ( ), and according to the formula Calculations are performed, including empirical coefficients. The value is typically set between 2 and 3. This method ensures that the threshold is outside the normal fluctuation range and can effectively identify significant and persistent abnormal deviations.

[0090] Setting the preset intermittent threshold: This threshold is used to capture sudden spike errors caused by transient disturbances (such as communication packet loss or short-term strong gusts). Its setting needs to combine engineering limits with historical transient anomaly data. First, set an absolute upper limit (e.g., 50% of the rated command value) based on the physical response limits of the actuator. Then, analyze confirmed transient anomalies in historical data and take the high percentile (e.g., the 99th percentile) of their error peak as a reference. The final threshold should be the lower of the two values ​​and dynamically adjusted according to communication link quality (e.g., packet loss rate) to ensure high sensitivity to high-risk transient events.

[0091] S3-1: Generate a sequence of historical anomaly control error amplitudes based on the historical anomaly environmental stress sub-cycle, and determine the control fusion period based on the historical anomaly control error amplitude sequence;

[0092] Based on the historical control error data corresponding to the historical joint anomaly dataset, the control error time series sequence aligned with the time of the M historical anomaly environmental stress sub-cycles is extracted, and the average amplitude of the control error in each sub-cycle is calculated to generate a historical anomaly control error amplitude sequence.

[0093] The duration of each historical abnormal environmental stress sub-cycle is calculated, and the Pearson correlation coefficient between it and the average amplitude of the corresponding historical abnormal control error amplitude sequence is calculated. The Pearson correlation coefficient is defined as the environmental-error correlation strength coefficient.

[0094] The control fusion period is determined based on the calculated environment-error correlation strength coefficient. The rule for determining the control fusion period is: the duration of the historical abnormal environmental stress sub-cycle corresponding to the absolute value of the environment-error correlation strength coefficient exceeding the preset correlation threshold is determined as the control fusion period.

[0095] It should be noted that in the ROS2-based intelligent assisted driving system for tugboats, determining the control fusion cycle is a crucial step in achieving adaptive control. First, control error data aligned with the time of M historical abnormal environmental stress sub-cycles is extracted; for example, the thrust error and rudder angle error sequences within each 15-minute sub-cycle are matched. Since the thrust error (unit: N) and rudder angle error (unit: ° or rad) have different physical dimensions, to avoid the dimensional differences affecting subsequent analysis, Z-score standardization of both types of error data is required before calculating the average error amplitude within each sub-cycle. ,in, This is the original error value. and These are the mean and standard deviation of the historical data for this type of error, respectively. The standardized error values ​​are dimensionless, and the combined average amplitude over the sub-periods is calculated. Calculate the average magnitude of the control error within each sub-cycle: ,in, This represents the total number of data points within a sub-period. Indicates the first The average error magnitude of each sub-cycle, for Control error at any given time. This value reflects the control accuracy of the system under specific environmental stresses.

[0096] By calculating the duration of the sub-cycle With average error magnitude Pearson correlation coefficient: , The correlation coefficient between duration and error magnitude. This represents the average duration of the sub-cycle. This represents the average value of the average error magnitude. This represents the total number of historical abnormal environmental stress sub-cycles. When the absolute value of the correlation coefficient exceeds a preset threshold (e.g., 0.7), it indicates that the duration of the environmental stress is significantly correlated with the control error. In this case, the duration of this sub-cycle is determined as the control fusion cycle. For example, if a 30-minute sub-cycle shows a strong correlation, then 30 minutes is used as the time reference for optimizing control command updates to ensure optimal control performance under similar environmental conditions.

[0097] It should be noted that this method first extracts control error data precisely aligned with M historical abnormal environmental stress sub-cycles. To eliminate the dimensional differences between thrust and rudder angle errors, Z-score standardization is applied first. Then, the average absolute value of the standardized error within each sub-cycle is calculated, generating a dimensionless sequence of historical abnormal control error amplitudes characterizing the degree of system performance degradation. Subsequently, the Pearson correlation coefficient between the duration of each abnormal sub-cycle and its corresponding average error amplitude is calculated to quantify the linear correlation between the duration of environmental stress and the magnitude of system control error. Finally, the durations corresponding to sub-cycles with correlation coefficients exceeding a preset threshold (e.g., 0.7) are determined as control fusion cycles. This allows the system to learn from historical experience and autonomously adjust the update cycle of control commands to the duration best matched to the current environmental disturbance mode. Therefore, when facing persistent disturbances, it avoids oscillations caused by excessively frequent control, effectively balancing the system's response speed and control stability, and significantly improving its adaptability and robustness in complex environments.

[0098] S3-2: Jointly analyze the deviation consistency and the control fusion cycle, and quantify them into a system reliability weighting factor;

[0099] The joint analysis formula is as follows: ,in, As a system reliability weighting factor, For consistency of deviation, To control the fusion cycle, and This is a preset weighted index used to adjust the contribution of the two indicators.

[0100] It should be noted that in the ROS2-based intelligent assisted driving system for tugboats, the system reliability weighting factor... Through joint analysis of deviation consistency and normalized control fusion cycle Key metrics calculated. Normalization was performed using... ,in This represents the largest integration cycle in history, ensuring dimensional uniformity. The calculation formula is: ,in and The preset weighting index (with a value of 1-2) is used for deviation consistency. (0-1) reflects the accuracy of coordinated control, after normalization. (0-1) characterizes the system's response capability to environmental stress. This joint analysis ensures that the system achieves high control accuracy and fast response speed. Value, for example when =0.9, =0.5 (corresponding to) =30 minutes =60 minutes) then take = =1, then =0.9×2=1.8. A higher value indicates stronger system reliability and supports more proactive obstacle avoidance strategies.

[0101] S4-1: Generate conservative obstacle avoidance paths and proactive obstacle avoidance paths based on the system reliability weighting factor;

[0102] The system reliability weighting factor is used as an input parameter and input into the local path planning algorithm of the sub-ship; the local path planning algorithm adopts the dynamic window method.

[0103] The system reliability weighting factor is used to dynamically adjust the cost function weight and sampling window size in the path planning algorithm;

[0104] The specific rules for dynamic adjustment are as follows: if the system reliability weight factor is low, the weight of obstacle distance cost is increased, and the range of the velocity sampling window is narrowed to generate a conservative obstacle avoidance path.

[0105] If the system reliability weight factor is high, the weights of path smoothness and schedule cost are increased, and the range of the speed sampling window is expanded to generate an active obstacle avoidance path.

[0106] It should be noted that in the ROS2-based intelligent assisted driving system for tugboats, the system reliability weight factor W directly affects the local path planning strategy. The Dynamic Window Method (DWA) is adopted as the core planning method. This method generates trajectories by sampling in the velocity space and evaluates the quality of the trajectories through a cost function.

[0107] When the system reliability weight factor is low (e.g., W < 1.0), it indicates poor control accuracy or environmental adaptability. In this case, the weight of obstacle distance cost is increased (e.g., from 0.4 to 0.7), while the velocity sampling window range is reduced (e.g., linear velocity is reduced from [0, 2 m / s] to [0, 1 m / s]), generating a conservative obstacle avoidance path. This path prioritizes ensuring a safe distance, sacrificing some travel efficiency.

[0108] When the system reliability weight factor is high (e.g., W>2.0), the weights of path smoothness cost (reducing steering jitter) and schedule cost (increasing speed) are increased, while the speed sampling window is expanded (e.g., [0, 3 m / s]), generating an active obstacle avoidance path. This path improves navigation efficiency and comfort while ensuring safety.

[0109] The specific form of the cost function is: ,in, The total cost is the cost function of path planning. The distance from the trajectory to the nearest obstacle. Cost of path smoothness (turning change). The cost of progress (time / distance to reach the target). , , These are the weights for obstacle distance, path smoothness, and progress cost, which are dynamically adjusted based on the W value. This adaptive mechanism enables intelligent switching of obstacle avoidance strategies under different reliability states. The three physical quantities in the cost function ( , , Because the dimensions are different, direct weighted summation will lead to ambiguity in the physical meaning of the cost. Therefore, before calculating the total cost, the three terms need to be minimized and maximally normalized to scale them to the dimensionless interval [0,1]. , , ,in, and These represent the minimum and maximum values ​​of each physical quantity within the current sampling window, respectively. After normalization, the cost function becomes: At this time, the weight , , This is a dimensionless adjustment coefficient, the magnitude of which directly reflects the relative importance of each cost in the decision-making process.

[0110] S4-2: Obtain a set of cooperative obstacle avoidance paths based on the conservative obstacle avoidance path and the active obstacle avoidance path, and convert the set of cooperative obstacle avoidance paths into corresponding thrust and rudder angle control commands.

[0111] The path deviation of the mother ship is calculated based on the expected trajectory of the mother ship and the real-time status data in the mother ship's operation data.

[0112] The degree of path deviation is quantified by calculating the Hausdorff distance between the actual track point sequence of the mother ship and the expected track point sequence;

[0113] A multi-objective optimization function is constructed with the goal of minimizing the path deviation of the mother ship as the global optimization objective and the obstacle avoidance paths generated by each of the sub-ships according to the system reliability weight factor as the constraints.

[0114] The multi-objective optimization function simultaneously considers the collision avoidance constraints and kinematic constraints between each sub-ship;

[0115] The multi-objective optimization function is solved using a distributed model predictive control algorithm to generate the final set of cooperative obstacle avoidance paths for each sub-ship.

[0116] The distributed model predictive control algorithm employs a rolling time-domain optimization strategy, resolving the optimization problem based on the latest system state in each control cycle;

[0117] The final set of cooperative obstacle avoidance paths for each sub-ship is converted into corresponding thrust and rudder angle control commands.

[0118] The command is sent to each of the sub-ships for execution via the ROS2 Action communication mechanism.

[0119] The control commands include longitudinal thrust commands and lateral rudder angle commands required for path tracking.

[0120] It should be noted that in the ROS2-based intelligent assisted driving system for tugboats, the generation of the cooperative obstacle avoidance path set is centered on tracking the mother ship's trajectory. The mother ship's expected trajectory is determined by mission planning, while the actual trajectory is constructed using real-time position, heading, and speed data acquired via GPS / INS. Path deviation is quantified using Hausdorff distance. ,in, This represents the Hausdorff distance (maximum path deviation). For set One of the points, For set One of the points, for arrive The maximum and minimum distances for arrive The maximum and minimum distances For point With point The Euclidean distance between them , These are the expected waypoint sequence and the actual waypoint sequence, respectively. This is the Euclidean distance. This distance reflects the maximum deviation between the mother ship's actual voyage and the expected path.

[0121] With minimizing path offset as the global objective and the obstacle avoidance paths of each sub-ship as constraints, a multi-objective optimization function is constructed, including collision avoidance constraints (distance between sub-ships ≥ safety threshold) and kinematic constraints (maximum steering angle). A distributed model predictive control (DMPC) algorithm is employed to solve the optimization problem in a rolling manner during each control cycle (e.g., 2 seconds), generating a set of cooperative obstacle avoidance paths that satisfy all constraints—that is, the optimal combination of motion trajectories for each sub-ship.

[0122] Finally, the path points are converted into thrust commands through inverse kinematics. ,in, The thrust command is Newtons (N). The mass of the ship is measured in kilograms (kg). acceleration in meters per second 2 (m / s 2 ), and rudder angle commands ,in, The rudder angle command is in radians (rad) or degrees (°). The length of the ship is meters (m). The turning angular velocity is expressed in radians per second (rad / s). The speed is measured in meters per second (m / s). These commands are issued through the ROS2Action mechanism to ensure that all sub-vessels execute control commands synchronously, achieving overall collaborative obstacle avoidance.

[0123] It should be noted that the expected trackpoint sequence is generated and provided by the upper-level mission planning system, and its acquisition is a multi-source data fusion and planning process. Specifically, based on the mother ship's navigation mission (e.g., from port A to port B), electronic chart (ENC) information, port and channel rules, information on fixed obstacles (e.g., shoals, reefs), and real-time traffic instructions from the VTS (Vessel Traffic Service) center, the system generates an optimal safe route from the starting point to the target point using a global path planning algorithm (e.g., A*, Dijkstra's algorithm). This route consists of a series of ordered waypoints with latitude and longitude coordinates; this sequence of waypoints is the expected trackpoint sequence. This sequence serves as an ideal reference path for the mother ship's navigation and is distributed to this system as a global target benchmark for assessing the degree of path deviation of the mother ship and for conducting tugboat coordinated control.

[0124] It should be noted that the distributed model predictive control algorithm, which performs a 're-solution of the optimization problem based on the latest system state' in each control cycle, has the following core steps: receiving and processing real-time environmental data from AIS and radar sensors; dynamically identifying and tracking the real-time position and velocity information of surrounding static and dynamic obstacles; updating the collision avoidance constraints in the optimization problem based on this real-time information; and predicting the trajectory of obstacles in the future time domain; finally, combining the latest states of the mother ship and each sub-ship, solving for the optimal cooperative control command that satisfies all safety and dynamic constraints. This process ensures the system's real-time response and safety assurance capabilities in complex and variable aquatic environments.

[0125] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A data and interaction method for a tugboat intelligent assisted driving system based on ROS2, characterized by: Includes the following steps: S1: Using a ship as the mother ship and a tugboat as the daughter ship, acquire mother ship data, daughter ship data, and external environment data; based on the mother ship data and the daughter ship data, acquire mother ship operation data and daughter ship operation data, and obtain the deviation consistency between the daughter ship and the mother ship according to the mother ship operation data and daughter ship operation data, including: acquiring time-aligned mother ship status data and daughter ship status data based on the mother ship operation data and daughter ship operation data; the mother ship status data and daughter ship status data include position data, heading data, and speed data; based on the time alignment... Using mother ship status data and daughter ship status data, construct mother ship status time series and daughter ship status time series; calculate the mother ship status change at adjacent time points based on the mother ship status time series to generate a mother ship deviation time series; calculate the daughter ship status change at adjacent time points based on the daughter ship status time series to generate a daughter ship deviation time series; perform time delay alignment processing on the mother ship deviation time series and the daughter ship deviation time series, calculate the Pearson correlation coefficient between the two series, and define the Pearson correlation coefficient as the deviation consistency between the daughter ship and the mother ship; S2: Obtain mother ship control data and daughter ship control data based on the mother ship data and the daughter ship data, and generate a control error time series based on the mother ship control data and the daughter ship control data; determine historical abnormal environmental stress sub-cycles based on the control error time series, including: extracting data points with persistence higher than a preset persistence threshold and data points with intermittency higher than a preset intermittency threshold based on the control error time series to form a historical joint anomaly dataset; the historical joint anomaly dataset is used to mark the specific time points when system anomalies occurred in history; extracting the duration of environmental stress exceeding a preset environmental threshold based on the historical environmental data corresponding to the time points of the historical joint anomaly dataset to form a historical abnormal environmental stress time period set; dividing the historical abnormal environmental stress time period set into M historical abnormal environmental stress sub-cycles according to a preset time granularity; S3: Generate a historical anomaly control error amplitude sequence based on the historical anomaly environmental stress sub-cycles, and determine the control fusion period based on the historical anomaly control error amplitude sequence, including: extracting the control error time series sequence aligned with the time of M historical anomaly environmental stress sub-cycles based on the historical control error data corresponding to the historical joint anomaly dataset, and calculating the average amplitude of the control error within each sub-cycle to generate a historical anomaly control error amplitude sequence; calculating the Pearson correlation coefficient between the duration of each historical anomaly environmental stress sub-cycle and the average amplitude of the corresponding historical anomaly control error amplitude sequence, and defining the Pearson correlation coefficient as the environment-error correlation strength coefficient; determining the control fusion period based on the calculated environment-error correlation strength coefficient; the rule for determining the control fusion period is: the duration of the historical anomaly environmental stress sub-cycle corresponding to the absolute value of the environment-error correlation strength coefficient exceeding a preset correlation threshold is determined as the control fusion period; jointly analyzing the deviation consistency and the control fusion period to quantify it as a system reliability weight factor, including: the joint analysis formula is as follows. ,in, As a system reliability weighting factor, For consistency of deviation, To control the fusion cycle, and This is a preset weighted index used to adjust the contribution of the two indicators; S4: Generate a conservative obstacle avoidance path and an active obstacle avoidance path based on the system reliability weight factor; obtain a cooperative obstacle avoidance path set based on the conservative obstacle avoidance path and the active obstacle avoidance path, and convert the cooperative obstacle avoidance path set into corresponding thrust and rudder angle control commands.

2. The data and interaction method for the ROS2-based intelligent assisted driving system for tugboats according to claim 1, characterized in that, The process of using a ship as the mother ship and a tugboat as the daughter ship to acquire mother ship data, daughter ship data, and external environment data includes: The mothership data is divided into mothership operation data and mothership control data; The sub-ship data is divided into sub-ship operation data and sub-ship control data; The external environment data is divided into historical environment data and real-time environment data.

3. The data and interaction method for a tugboat intelligent assisted driving system based on ROS2 according to claim 1, characterized in that, The step of obtaining mother ship control data and daughter ship control data based on the mother ship data and the daughter ship data, and generating a control error time series based on the mother ship control data and the daughter ship control data, includes: Based on the mother ship control data and the daughter ship control data, obtain time-aligned mother ship control commands and daughter ship control feedback. The mother ship control commands include thrust commands and rudder angle commands, and the daughter ship control feedback includes thrust feedback and rudder angle feedback. Based on the time-aligned mother ship control commands and daughter ship control feedback, calculate the command tracking error at the same moment and generate a control error timing sequence. The command tracking error is the difference between the sub-ship control feedback and the mother ship control command.

4. The data and interaction method for a tugboat intelligent assisted driving system based on ROS2 according to claim 1, characterized in that, The generation of conservative obstacle avoidance paths and proactive obstacle avoidance paths based on the system reliability weighting factor includes: The system reliability weighting factor is used as an input parameter and input into the local path planning algorithm of the sub-ship; the local path planning algorithm adopts the dynamic window method. The system reliability weighting factor is used to dynamically adjust the cost function weight and sampling window size in the path planning algorithm; The specific rules for dynamic adjustment are as follows: if the system reliability weight factor is low, the weight of obstacle distance cost is increased, and the range of the velocity sampling window is narrowed to generate a conservative obstacle avoidance path. If the system reliability weight factor is high, the weights of path smoothness and schedule cost are increased, and the range of the speed sampling window is expanded to generate an active obstacle avoidance path.

5. The data and interaction method for a tugboat intelligent assisted driving system based on ROS2 according to claim 1, characterized in that, The step of obtaining a cooperative obstacle avoidance path set based on the conservative obstacle avoidance path and the active obstacle avoidance path, and converting the cooperative obstacle avoidance path set into corresponding thrust and rudder angle control commands, includes: The path deviation of the mother ship is calculated based on the expected trajectory of the mother ship and the real-time status data in the mother ship's operation data. The degree of path deviation is quantified by calculating the Hausdorff distance between the actual track point sequence of the mother ship and the expected track point sequence; A multi-objective optimization function is constructed with the goal of minimizing the path deviation of the mother ship as the global optimization objective and the obstacle avoidance paths generated by each of the sub-ships according to the system reliability weight factor as the constraints. The multi-objective optimization function simultaneously considers the collision avoidance constraints and kinematic constraints between each sub-ship; The multi-objective optimization function is solved using a distributed model predictive control algorithm to generate the final set of cooperative obstacle avoidance paths for each sub-ship. The distributed model predictive control algorithm employs a rolling time-domain optimization strategy, resolving the optimization problem based on the latest system state in each control cycle; The final set of cooperative obstacle avoidance paths for each sub-ship is converted into corresponding thrust and rudder angle control commands.

6. The data and interaction method for a tugboat intelligent assisted driving system based on ROS2 according to claim 5, characterized in that, The process of converting the final set of cooperative obstacle avoidance paths for each sub-ship into corresponding thrust and rudder angle control commands includes: The command is sent to each of the sub-ships for execution via the ROS2 Action communication mechanism. The control commands include longitudinal thrust commands and lateral rudder angle commands required for path tracking.

Images