Ship berthing pose estimation method and system based on observation confidence self-calibration

Abstract

The application discloses a ship berthing pose estimation method and system based on observation confidence self-calibration, and relates to the technical field of intelligent perception and collaborative operation of ships. The method comprises the following steps: obtaining multi-source observation information among a plurality of tugboats, an assisted ship and a target berth; using a multi-source observation model to perform inertial observation and corresponding motion compensation on different multi-source observation information; performing consistency evaluation on the confidence of the multi-source observation information based on self-calibration, and performing adaptive weighted fusion; optimizing the inertial observation process based on the fused pose constraint information to obtain consistent relative pose estimation of each tugboat. The application can solve the operation pain points of different ports, realize online consistency discrimination, confidence calculation and historical confidence self-calibration of multi-modal observation, and complete adaptive weighting and distributed collaborative updating under the condition that port communication is limited, so as to output stable and consistent relative pose and uncertainty.

Description

Technical Field

[0001] This invention relates to the field of ship intelligent sensing and collaborative operation technology, and in particular to a method and system for estimating ship berthing posture based on observation confidence self-calibration. Background Technology

[0002] The statements in this section are merely background information related to the present invention and do not necessarily constitute prior art.

[0003] Large vessel berthing operations typically require multiple tugboats working together to provide lateral thrust and bow / stern attitude adjustment. The operating environments of different types of ports vary significantly. For example, container ports face challenges such as high-density container stacking and strong obstruction caused by frequent movement of quay cranes; bulk cargo ports experience significant dust generation during loading and unloading operations, which can cause sensor data scattering and distortion; and petrochemical ports have stringent requirements for equipment explosion-proof and electromagnetic compatibility, and prohibit the operation of non-explosion-proof electrical equipment.

[0004] Meanwhile, port environments are generally characterized by strong shading, high reflectivity, multipath propagation, and significant wave fluctuations and wind and turbulence, resulting in continuous changes in the relative positions and attitudes of tugboats, ships, and berths. Existing solutions often use single tugboats or single-modal sensors for measurement, which are prone to outlier observations under conditions of shading, missing markers, or multipath propagation. Some solutions employ centralized fusion architectures, which are susceptible to delays and single-point failures when port communication bandwidth is limited, links are unstable, or central nodes fail. Furthermore, during multi-tugboat coordinated berthing, the tugboat formation and topology change with the operational phase: the addition or removal of tugboats, baseline expansion and contraction, and relative rotation cause changes in observation redundancy and observability at any time. If the observation weights and fusion parameters are fixed, outlier observations will be erroneously amplified and propagated to the global estimate, causing accumulated relative pose drift, which in turn affects thrust distribution and berthing safety.

[0005] Therefore, due to the complexity and uncertainty of the multi-source collaborative process of large ship berthing, there is currently a lack of online consistency assessment for the observation edge of each tugboat to the ship, berth or tugboat, and existing technical solutions cannot generate observation confidence that can be adaptively adjusted and traced, thus affecting the accuracy of the pose determination of large ship berthing process and the degree of flexible matching for different port operations. Summary of the Invention

[0006] To address the shortcomings of existing technologies, the purpose of this invention is to provide a method and system for estimating ship berthing pose based on observation confidence self-calibration. This system can address the operational challenges of different ports, such as container ports, bulk cargo ports, and petrochemical ports, by enabling online consistency judgment, confidence calculation, and historical confidence self-calibration of multimodal observations. Furthermore, it can perform adaptive weighting and distributed collaborative updates under port communication constraints, thereby outputting a stable and consistent relative pose and its uncertainty.

[0007] To achieve the above objectives, the present invention is implemented through the following technical solution:

[0008] The first aspect of this invention provides a method for estimating ship berthing pose based on observation confidence self-calibration, comprising the following steps:

[0009] Acquire multi-source observation information from multiple tugboats, assisted vessels, and target berths, and perform coordinate unification and time alignment operations on the multi-source observation information;

[0010] A multi-source observation model is constructed, and inertial observation and corresponding motion compensation are performed on different multi-source observation information using the multi-source observation model.

[0011] Consistency assessment is performed on multi-source observation information based on self-calibrated confidence, and adaptive weighted fusion is performed on the multi-source observation information that passes the consistency assessment. Event-triggered communication and edge selection strategies are executed based on the adaptive weighted fusion results to obtain the fused pose constraint information.

[0012] The inertial observation process is optimized based on the fused pose constraint information to obtain a consistent relative pose estimate for each tugboat.

[0013] Furthermore, the specific steps for performing coordinate unification and time alignment on multi-source observation information include:

[0014] The data acquisition devices were calibrated separately.

[0015] Synchronize the time of all data acquisition devices;

[0016] A four-level coordinate system was established, which was linked to the port surveying benchmark, and all multi-source observation information was unified in coordinates.

[0017] Furthermore, the multi-source observation model includes a visual observation model, an ultra-wideband (UWB) observation model, and a lidar observation model. The visual observation model is used to visually observe the marking images of the hull or berth to obtain the relative pose of the tugboat to the ship; the UWB observation model is used to solve the position of the tugboat based on the signal transmission time; and the lidar observation model is used to cluster and segment the lidar point cloud to obtain the relative pose.

[0018] Furthermore, the specific steps for conducting a consistency assessment of multi-source observation information based on self-calibrated confidence levels are as follows:

[0019] Normalized statistics are calculated using the observation residuals and covariance of the observation residuals from multi-source observation information;

[0020] A consistency test strategy based on two thresholds is used to evaluate the consistency of normalized statistics and obtain a consistency score.

[0021] The consistency score is self-calibrated, and the confidence level is calculated in combination with the port scenario.

[0022] Furthermore, the specific steps for self-calibrating the consistency score and calculating the confidence level in conjunction with the port scenario are as follows:

[0023] The historical reputation self-calibration model is used to calculate the reputation value of observation information under different modes based on the consistency score;

[0024] The confidence level is calculated by integrating reputation score, consistency score and port-specific environmental risk factors.

[0025] Furthermore, the specific steps for executing event-triggered communication and edge selection strategies based on the adaptive weighted fusion results are as follows:

[0026] Set the event trigger conditions and the execution conditions for the edge selection strategy;

[0027] The adaptive weighted fusion result is dynamically reconstructed based on the event triggering conditions and the edge selection strategy execution conditions.

[0028] Furthermore, based on the fused pose constraint information, a port-specific degradation and blind spot compensation strategy is set up during the inertial observation process. When the port-specific degradation and blind spot compensation strategy is triggered, if there are redundant sensors, the observation data of the redundant sensors will be automatically switched; if there are no redundant sensors, the blind spot compensation data of the port shore-based facilities will be called.

[0029] A second aspect of the present invention provides a ship berthing pose estimation system based on observation confidence self-calibration, comprising:

[0030] The data acquisition module is configured to acquire multi-source observation information from multiple tugboats, assisted vessels, and the target berth, and to perform coordinate unification and time alignment operations on the multi-source observation information.

[0031] The inertial observation module is configured to build a multi-source observation model and use the multi-source observation model to perform inertial observation and corresponding motion compensation on different multi-source observation information.

[0032] The consistency assessment and information fusion module is configured to perform consistency assessment on multi-source observation information based on self-calibrated confidence, and to perform adaptive weighted fusion on multi-source observation information that has passed the consistency assessment. Based on the adaptive weighted fusion result, it executes event-triggered communication and edge selection strategies to obtain the fused pose constraint information.

[0033] The relative pose estimation module is configured to optimize the inertial observation process based on the fused pose constraint information to obtain a consistent relative pose estimate for each tugboat.

[0034] A third aspect of the present invention provides a computer-readable storage medium storing a computer program adapted for loading by a processor and executing steps in the ship berthing attitude estimation method based on observation confidence self-calibration as described in the first aspect of the present invention.

[0035] A fourth aspect of the present invention provides a computer device comprising:

[0036] A processor, adapted to execute computer programs;

[0037] A computer-readable storage medium storing a computer program, which, when executed by the processor, implements the ship berthing attitude estimation method based on observation confidence self-calibration as described in the first aspect of the present invention.

[0038] The above one or more technical solutions have the following beneficial effects:

[0039] This invention discloses a method and system for estimating ship berthing pose based on observation confidence self-calibration. It can perform online consistency assessment of the observation edges from each tugboat to the ship, berth, or tugboat, generating traceable observation confidence levels. Based on the confidence level, it drives adaptive covariance adjustment and event-triggered communication scheduling, suppressing the impact of outliers and incomplete information on distributed cooperative localization and pose determination from the source. It also adapts to the operational characteristics of different ports. This invention is designed for multi-tugboat cooperative assisted berthing operations of large ships in various types of ports, including container ports, bulk cargo ports, and petrochemical ports. It can achieve multi-source relative pose observation confidence assessment, self-calibration, and adaptive fusion. It can be used to construct highly reliable relative pose constraint edges between tugboats, ships, and berths, and output relative pose references with uncertainty to thrust distribution, cooperative maneuvering, and berthing safety control, while also being compatible with the regulatory requirements of the Port Vessel Traffic Service (VTS).

[0040] The method of this invention can provide observation edge confidence and traceability weights from the source, and effectively suppress the propagation of outlier errors caused by obstruction, dust and electromagnetic interference in different port scenarios through two-level threshold testing.

[0041] This invention also introduces a historical reputation self-calibration model, which enables sensors and links to adaptively learn reliability during long-term operation. It accelerates the reputation decay of faulty sensors in special scenarios such as petrochemical ports, reducing the workload of manual parameter tuning.

[0042] The confidence-driven covariance scaling and weighted least squares fusion in this invention have a clear mathematical model and implementation path, which makes it easy to embed on edge computing units and adapt to the sensor configuration requirements of different ports.

[0043] The event-triggered communication and topology dynamic reconstruction strategy of this invention maintains key constraint updates under conditions of limited port communication bandwidth and link obstruction, improves system real-time performance and robustness, and prioritizes port shore-based nodes as relays to solve communication obstruction problems in scenarios such as container ports.

[0044] The method of this invention can output a relative pose reference with uncertainty, providing quantitative feedback for tugboat thrust distribution and coordinated operation, while being compatible with port VTS monitoring systems to meet maritime regulatory needs.

[0045] This invention addresses the operational pain points of different types of ports, such as container ports, bulk cargo ports, and petrochemical ports, by achieving adaptive adaptation of sensors and fusion strategies. It solves industry-specific pain points and improves the safety and accuracy of berthing operations at different ports.

[0046] Advantages of additional aspects of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0047] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0048] Figure 1 This is a flowchart of the ship berthing pose estimation method based on observation confidence self-calibration in Embodiment 1 of the present invention. Detailed Implementation

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

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

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

[0052] Example 1:

[0053] Embodiment 1 of this invention provides a method for estimating ship berthing pose based on observation confidence self-calibration, such as... Figure 1 As shown, the process includes steps such as multi-source observation acquisition, coordinate unification and time alignment, motion compensation prediction, consistency assessment and confidence generation, adaptive weighting, event-triggered communication and edge selection, and distributed collaborative estimation output.

[0054] Specifically, the following steps are included:

[0055] Step 1: Acquire multi-source observation information from multiple tugboats, assisted vessels, and the target berth, and perform coordinate unification and time alignment operations on the multi-source observation information.

[0056] Step 1.1: Acquire multi-source observation information among multiple tugboats, assisted vessels, and the target berth.

[0057] In one specific implementation, differentiated sensors and observation sources are configured for different port scenarios to acquire multi-source observation information.

[0058] Step 1.1.1: The tugboat end acquires data through multi-source sensors. The multi-source sensors include general-purpose sensors and port-specific sensors.

[0059] General-purpose sensors are used for visual observation, ranging or angle measurement, and inertial observation. Visual inspection data consists of images of the ship's hull or berth markings captured by cameras. Ranging or angle measurement data includes distance and angle of arrival output from UWB, distance values ​​output from laser ranging, distance and corresponding velocity and azimuth angle output from millimeter-wave radar, and lidar point cloud data. Inertial observation data includes three-axis angular velocities and accelerations output from an inertial measurement unit (IMU), heading angles output from a compass, and absolute position and velocity output from a Global Navigation Satellite System (GNSS).

[0060] Port-specific sensors include infrared cameras for container ports; dust-resistant coated lidar for bulk cargo ports; and explosion-proof UWB transceivers for petrochemical ports.

[0061] Step 1.1.2: Obtain reference information for shore base, ship hull, and port.

[0062] The hull reference information includes visual markings, coordinates, and millimeter-wave reflective marking characteristic parameters of key parts of the hull, including the bow, stern, and sides.

[0063] Berth reference information includes the geometric characteristic parameters of the fenders or mooring bollards, the fixed coordinates of the shore-based UWB base station, and the port geodetic coordinates of the berth reference point.

[0064] Port environmental monitoring information includes real-time shielding levels, dust concentrations, electromagnetic interference intensity, berthing area surge amplitudes, and tidal current velocity data provided by the port's VTS system.

[0065] Step 1.2: Perform coordinate unification and time alignment operations on the multi-source observation information.

[0066] Step 1.2.1: Calibrate the data acquisition devices respectively.

[0067] In one specific implementation, hand-eye calibration is performed on the camera and IMU of each tugboat. The Zhang calibration method is used to obtain the camera intrinsic parameter matrix and distortion coefficients, and the rotation matrix of the camera and IMU is solved by a motion-based calibration method. Translation vector Spatial extrinsic parameter calibration was performed on ranging sensors such as UWB and lidar, and the IMU was determined to establish the transformation relationship between the sensor coordinate system and the tugboat carrier coordinate system.

[0068] The time delay calibration of heterogeneous sensors is performed using the timestamp cross-correlation method, measuring the time offset of data from vision, ranging, and inertial sensors. The offset is then stored in the sensor parameter library. In the petrochemical port scenario, the time delay compensation coefficient of the explosion-proof sensor is additionally calibrated to eliminate the influence of the explosion-proof enclosure on signal transmission.

[0069] Step 1.2.2: Synchronize the time of all data acquisition devices.

[0070] In one specific implementation, the PTP precision time signal from the port's time synchronization server is introduced to uniformly synchronize the timestamps of all sensors to Coordinated Universal Time (UTC), with the synchronization error controlled within 1ms. For heterogeneous data collected at different frequencies, linear interpolation is used to interpolate the timestamps of low-frequency data (such as GNSS, 1Hz) to those of high-frequency data (such as IMU, 200Hz). During peak operating periods at container and bulk cargo ports, a redundancy verification mechanism for the time synchronization signal is activated to avoid interference from quay cranes and loading / unloading equipment.

[0071] Step 1.2.3: Establish a four-level coordinate system, associate it with the port surveying datum, and unify the coordinates of all multi-source observation information.

[0072] In one specific implementation, a four-level coordinate system is established, linked to the port surveying datum: the world coordinate system. (Fixed at the berth reference point), port geodetic coordinate system (Achieving communication with the port through real-time dynamic differential (RTK) base stations) Transformation of systems, transformation matrix (Official data calibration provided by the port surveying department) hull coordinate system (Fixed to the ship's center of gravity), tugboat coordinate system (Fixed at the first) (The center of gravity of the tugboat).

[0073] All observation data are unified to the world coordinate system through the following transformation relationship. middle:

[0074] ,

[0075] .

[0076] in, The coordinates of the observation point in the world coordinate system. This is the transformation matrix from the geodetic coordinate system to the world coordinate system. This is the transformation matrix from the ship's coordinate system to the geodetic coordinate system. The coordinates of the original observation points in the ship's coordinate system are used to unify the ship's own observation data (such as feature points on the ship) into the world coordinate system for the estimation of the relative pose between the tugboat and the ship. Let be the transformation matrix from the sensor coordinate system to the geodetic coordinate system used by the i-th tugboat. For the first The purpose of this transformation is to unify the observation point coordinates in the sensor coordinate system used by each tugboat into the world coordinate system, thereby estimating the relative pose between the tugboat and the berth, and between tugboats themselves. All observation data is unified to the world coordinate system through this transformation. In the context of petrochemical ports, additional checks are added to verify the coordinates of explosion-proof areas and restricted zones to prevent tugboats from entering dangerous areas.

[0077] Step 2: Construct a multi-source observation model and use the multi-source observation model to perform inertial observation and corresponding motion compensation on different multi-source observation information.

[0078] Step 2.1: Construct a multi-source observation model.

[0079] In one specific implementation, the multi-source observation model includes a visual observation model, a UWB observation model, and a lidar observation model. The visual observation model is used to visually observe the marking images of the hull or berth to obtain the relative pose of the tugboat to the ship. The UWB observation model is used to solve the position of the tugboat based on the signal transmission time. The lidar observation model is used to cluster and segment the lidar point cloud to obtain the relative pose.

[0080] This embodiment establishes a short-time kinematic model of the tugboat based on IMU and gyroscope data to predict its pose changes over a short period. Building upon this, corresponding observation models are constructed for three sensors: vision, UWB (ultra-wideband), and lidar, forming a pose estimation framework that combines kinematic prediction and multi-source observation correction. A constant velocity-constant angular velocity (CVCA) model is used, with the state vector defined as:

[0081] .

[0082] in, For state vectors, For location, For speed, For attitude angle, ω is the angular velocity.

[0083] The state vector is predicted by Kalman filtering to obtain... Predicting the relative pose prior at time and predicted covariance To address port surge and tidal disturbances, a disturbance compensation term is added. This compensation item is calculated from real-time data from shore-based surge monitoring stations.

[0084] Step 2.2: Use the multi-source observation model to perform inertial observation and corresponding motion compensation on different multi-source observation information.

[0085] In one specific implementation, for timestamps of Observational data The results are obtained through extrapolation or interpolation using short-time kinematic models. Time-compensated observations The calculation method is as follows:

[0086] .

[0087] This reduces measurement lag errors caused by relative motion, surges, and currents; in container port scenarios, it also provides additional compensation for short-term obstruction disturbances caused by the movement of quay crane spreaders.

[0088] Step 3: Perform a consistency assessment on the multi-source observation information based on the self-calibrated confidence level, and perform adaptive weighted fusion on the multi-source observation information that has passed the consistency assessment. Based on the adaptive weighted fusion result, execute event-triggered communication and edge selection strategies to obtain the fused pose constraint information.

[0089] Step 3.1: Conduct a consistency assessment of the multi-source observation information based on the self-calibrated confidence level.

[0090] Step 3.1.1: Calculate the normalized statistic using the observation residuals and covariance of the observation residuals from multi-source observation information.

[0091] In one specific implementation, for the first Construct measurement functions from observation data of various modalities (including visual, ranging, and point cloud modalities). Mapping state variables to observed values ​​and calculating observation residuals. and the covariance of the residuals :

[0092] ,

[0093] .

[0094] in, This represents the measured observation value of the m-th modality (including visual, ranging, and point cloud modalities) at the current time. In this embodiment, Let be the measured observation value at the current time t, and the value mentioned above... For any time The same modal observation data needs to be converted into compensated observations of the same dimension as at time t using a short-time kinematic model. hour, ,when hour, In this embodiment, time alignment compensation is required using a short-time kinematics model to convert the result to the desired time alignment. t Compensated observations in the same dimension at the same time Ultimately used with Participate in the calculation or fusion update of observation residuals; The tugboat pose state vector is predicted by the kinematic model based on the state estimation of the previous moment. To be based on the predicted state, through the first m Predicted observations calculated using modal measurement functions; observation residuals This represents the deviation between the measured and predicted values. The covariance of the residuals. This reflects the degree of uncertainty in the residuals, which can be broken down into two parts: uncertainty from the motion prediction model and observation noise from the sensor itself. The Jacobian matrix of the measurement function, To observe the covariance.

[0095] Calculate the normalized statistic:

[0096] .

[0097] Among them, normalized statistics The larger the value, the greater the deviation between the observed value and the predicted value.

[0098] Step 3.1.2: Based on the two-level threshold consistency test strategy, the normalized statistic is evaluated for consistency to obtain the consistency score.

[0099] In one specific implementation, the consistency verification strategy for the two-level threshold includes hard threshold verification and soft threshold verification.

[0100] Specifically, the hard threshold test uses the chi-square test, and a significance level is set. The hard threshold is obtained by querying the chi-square distribution table. .like If the observation is determined to be an outlier, it is discarded directly or its weight is reduced to below 0.1. In the container port scenario, the hard threshold of the visual sensor is reduced by 20% to improve the sensitivity of identifying occluded outliers.

[0101] The soft threshold test calculates the consistency score for observations that pass the hard threshold test using a logit mapping. The formula is:

[0102] .

[0103] in Soft threshold ( ), To adjust the coefficient, The value range is [0,1], and the larger the value, the higher the consistency; in the bulk cargo port scenario, the lidar's... The value was increased by 30% to enhance the ability to distinguish residuals caused by dust interference.

[0104] Step 3.1.3: Perform self-calibration on the consistency score and calculate the confidence level in combination with the port scenario.

[0105] Step 3.1.3.1: Calculate the reputation value of the observation information under different modes based on the consistency score using the historical reputation self-calibration model.

[0106] In one specific implementation, a historical reputation self-calibration model is constructed, and the reputation value iteration formula is as follows:

[0107] .

[0108] in For the first time The reputation value of modal sensors, The decay factor (values ​​from 0.1 to 0.3, with 0.1 used in the petrochemical port scenario to accelerate the decay of faulty sensor reputation). For the first Consistency score at any given moment.

[0109] Extracting Port-Specific Environmental Risk Factors Visual sensors in container port obstruction scenarios LiDAR in high-dust environments at bulk cargo ports UWB Sensors in Strong Electromagnetic Interference Scenarios at Petrochemical Ports .

[0110] Step 3.1.3.2: Calculate the confidence level by integrating reputation score, consistency score and port scenario-based environmental risk factors.

[0111] In one specific implementation, the final observation confidence level is obtained:

[0112] .

[0113] in, To observe the confidence level, , , These are the weight coefficients for the corresponding items. The weighting coefficients are adaptively adjusted according to the port scenario.

[0114] Step 3.2: Perform adaptive weighted fusion on the multi-source observation information that has passed the consistency assessment.

[0115] Step 3.2.1: Scaling the observation covariance based on confidence level.

[0116] In one specific implementation, according to The observed covariance is scaled using the following formula:

[0117] .

[0118] in, This represents the scaled observation covariance. Higher confidence levels result in smaller scaled covariance and higher weighting of the observation data. In the petrochemical port scenario, the covariance scaling factor for the explosion-proof sensor is additionally multiplied by 1.2 to compensate for measurement errors in the explosion-proof enclosure.

[0119] Step 3.2.2: Adaptive weighted fusion based on weighted least squares method.

[0120] In one specific implementation, a fusion optimization objective function is constructed:

[0121] .

[0122] Each observation edge l belongs to a certain observation mode m (visual, UWB, lidar), and the corrected covariance of observation edge l is... The corrected covariance is directly inherited from its mode m. (Right now This enables the transfer of modal-level reliability assessment to the observation-level.

[0123] The optimization problem is solved using an iterative weighted least squares method, resulting in a set of constraint information for three types of pairwise node pairs: tugboat-ship, tugboat-berth, and tugboat-tugboat, i.e., the fused relative pose constraints. and constrained covariance Simultaneously calculate the information contribution index for each observation edge. ,in, Represents the trace operation of a matrix. The larger the value, the greater the contribution of the observation edge to the fusion result; in the container port scenario, high-contribution observation edges of UWB and millimeter-wave radar are preferentially retained.

[0124] Step 3.3: Execute event-triggered communication and edge selection strategies based on the adaptive weighted fusion results.

[0125] Step 3.3.1: Set the event triggering conditions and the edge selection strategy execution conditions.

[0126] In one specific implementation, event triggering conditions are set. The information gain at adjacent time points is calculated.

[0127] .

[0128] in, Let be the constrained covariance matrix of the fused output at time t, which is the uncertainty measure of the pose estimation result at time t; The constrained covariance is the variance between adjacent time points (t-1), i.e., the posterior covariance after fusion and updating at time t-1. Information gain between adjacent time steps; The KL divergence is used to measure the difference between the two fusion results; the rate of change of edge contribution is also calculated.

[0129] .

[0130] in, The contribution index to the information at time t. The contribution index to information at adjacent time points.

[0131] Rate of change of confidence level:

[0132] .

[0133] in, The rate of change of confidence level. Let be the observation confidence level at time t. The confidence level of observations at adjacent time points.

[0134] This embodiment sets a port-specific trigger threshold: peak container port operations. Reduce by 15% to decrease communication frequency; dangerous areas at petrochemical ports. Increase by 20% to improve communication sensitivity. When the requirements are met... or When this occurs, neighborhood communication is triggered. Among these, A preset threshold for determining the information gain of the system is used to judge the overall convergence state of the fusion system. A preset threshold for determining the change in observation confidence is used to judge the observation reliability of a single observation edge.

[0135] In one specific implementation, the execution conditions for the edge selection strategy are set. Under bandwidth budget constraints, a greedy algorithm is used to select the information contribution index. The top N observed edges (N is determined by bandwidth capacity) only send incremental information about these edges to neighboring nodes. , Let be the pose increment vector and covariance increment matrix between time t and time t-1, respectively. By sending only the pose increment and covariance increment to neighboring nodes, instead of the complete pose state and covariance matrix, the amount of communication data can be effectively reduced, achieving efficient distributed information exchange within the bandwidth budget. In the bulk cargo port scenario, the observation edge of millimeter-wave radar is preferred due to its strong resistance to dust interference and high information stability.

[0136] Step 3.3.2: Perform topology dynamic reconstruction on the adaptive weighted fusion result based on the event triggering conditions and the edge selection strategy execution conditions.

[0137] In one specific implementation, the signal-to-noise ratio (SNR) and packet loss rate (PLR) of the inter-tugboat communication link are monitored in real time, and a link quality threshold is set. , ,in, This represents the signal-to-noise ratio threshold for the communication link between tugboats. This represents the packet loss rate threshold. If a certain link... or This determines the link quality degradation.

[0138] Simultaneously, a node search algorithm is initiated. Based on the topology information of the port's shore-based communication base stations, the built-in confidence of the search neighborhood is determined. Temporary communication links are established using tugboat nodes or shore-based nodes; in container port scenarios, shore-based nodes are preferred as relays to penetrate communication obstructions caused by container stacking.

[0139] This embodiment aims to maximize information gain by optimizing the temporary topology and ensuring the stability of the communication link.

[0140] Step 4: Optimize the inertial observation process based on the fused pose constraint information to obtain a consistent relative pose estimate for each tugboat.

[0141] In one specific implementation, the high-precision pose constraint information obtained by the aforementioned multi-source fusion method is used to correct the error accumulation problem in the inertial observation process, and finally achieve consistent estimation of the relative pose of each tugboat.

[0142] The specific steps are as follows:

[0143] Step 4.1: Optimize the inertial observation process based on the fused pose constraint information.

[0144] In one specific implementation, each tugboat will fuse its pose constraint information, such as the information matrix of the i-th tugboat. (Representing the accuracy and determinism of the pose estimation of the i-th tugboat), information vector Wait, send to neighboring nodes, It is the inverse of the covariance matrix of the i-th tugboat after fusion. The relative pose constraint for the i-th tugboat.

[0145] The weighted average consensus iterative algorithm is adopted, and the iterative formula is as follows:

[0146] ,

[0147] .

[0148] Where i represents the i-th tugboat, j represents the j-th tugboat in its neighborhood, and k represents the round of the consistency iteration, starting from the initial value k=0. , Let these represent the information matrices of the i-th and j-th tugboats at the k-th iteration, respectively. This represents the information matrix of the i-th tugboat in the (k+1)-th iteration. , Let i be the information vector of the i-th and j-th tugboats at the k-th iteration. This is the information vector of the i-th tugboat at the (k+1)-th iteration. The fusion weight of the information of the i-th tugboat to the j-th tugboat in the neighborhood is usually determined by the communication topology or information contribution, N i Let be the set of neighborhood nodes of the i-th tugboat. Repeat the iteration until the convergence condition is met. .

[0149] in, The convergence threshold is set to 10. 6 .

[0150] At this point, a consistent relative pose estimate of each tugboat is obtained. :

[0151] .

[0152] and consistent covariance :

[0153] .

[0154] Step 4.2: Based on the fused pose constraint information, set up a port-specific degradation and blind spot compensation strategy during the inertial observation process.

[0155] In one specific implementation, when the confidence of an observation edge remains below 0.3 for T seconds (T is 5 to 10 seconds, 5 seconds is taken in the petrochemical port scenario to speed up emergency response), or when the rank of the observability matrix is ​​less than the state dimension, a port-specific degradation and blind spot filling strategy is triggered.

[0156] When the port-specific downgrade and blind spot filling strategy is triggered, if there are redundant sensors, the system will automatically switch to the observation data of the redundant sensors; if there are no redundant sensors, the system will call the blind spot filling data of the port's shore-based facilities.

[0157] Specifically, container ports use visual sensor data from quay cranes; bulk cargo ports use millimeter-wave radar data linked to dust monitoring stations; and petrochemical ports use RTK base station data from explosion-proof areas to correct the tugboat's pose and maintain the accuracy of relative pose estimation.

[0158] The system ultimately outputs consistent relative pose, pose uncertainty, sensor health status, and communication topology information of each tugboat relative to the ship or berth, and synchronizes it to the port VTS system to meet maritime regulatory requirements.

[0159] To better illustrate the method of this embodiment, see the accompanying drawings. Figure 1 An example will be provided. The specific steps are as follows:

[0160] S1: Multi-source observation and acquisition.

[0161] Tugboat-side sensing and computing units: Each tugboat is equipped with differentiated sensors. Container ports are equipped with visible light and infrared dual cameras, UWB transceivers, and LiDAR; bulk cargo ports are equipped with dust-resistant coated LiDAR and millimeter-wave radar; and petrochemical ports are equipped with explosion-proof UWB transceivers, explosion-proof millimeter-wave radar, and intrinsically safe IMUs. All tugboats are equipped with edge computing units (such as NVIDIA Jetson AGX) for online confidence calculation, fusion, and communication scheduling.

[0162] Ship-side reference identification unit: AprilTag visual identifiers and millimeter-wave reflective identifiers are installed on key parts of the hull, such as the bow, stern, and sides. The coordinates of the identifiers are stored in the system after being calibrated by a total station. Ships in petrochemical ports are additionally equipped with explosion-proof identification plates for sensor identification.

[0163] Berth-end or shore-based reference facilities: Fixed UWB base stations and RTK base stations are deployed in the berth area to collect geometric feature data of fenders and mooring bollards; shore-based port environmental monitoring units are deployed to collect data such as shielding level (container port), dust concentration (bulk cargo port), and electromagnetic interference intensity (petrochemical port) in real time; berths in petrochemical ports are additionally equipped with explosion-proof area electronic fence sensors.

[0164] Communication Unit: Tugboats are equipped with 5G or self-organizing network communication modules to support neighborhood communication between tugboats and between tugboats and shore bases. The communication protocol adopts UDP protocol to reduce transmission latency. In the petrochemical port scenario, the communication module meets explosion-proof requirements and adopts frequency bands that are resistant to electromagnetic interference.

[0165] S2: Coordinate unification and time alignment.

[0166] World coordinate system The origin is located at the center reference point of the berth, the X-axis points towards the port channel, the Y-axis is perpendicular to the X-axis and points towards the inside of the berth, and the Z-axis is perpendicular to the horizontal plane and points upward.

[0167] Hull coordinate system The origin is located at the ship's center of gravity, the X-axis points to the bow of the ship, the Y-axis points to the starboard side of the ship, and the Z-axis is perpendicular to the ship's deck and pointing upwards.

[0168] Tug coordinate system The origin is located at the th The center of gravity of the tugboat is defined by the X-axis pointing towards the bow, the Y-axis pointing towards the starboard side, and the Z-axis perpendicular to the deck and pointing upwards.

[0169] The transformation relationships between the various coordinate systems are updated in real time through external parameter calibration and kinematic model, and are precisely associated with the port geodetic coordinate system, meeting the accuracy requirements of port surveying and supervision.

[0170] S3: Motion compensation prediction.

[0171] (1) Visual observation model.

[0172] The tugboat camera captures images of the AprilTag markers on the hull or berth, and the transformation matrix from the camera coordinate system to the marker coordinate system is solved using the PnP algorithm. Combined with camera external parameters (Transformation matrix from tugboat coordinate system to camera coordinate system) yields the relative pose of the tugboat and the ship. :

[0173] .

[0174] in This is the transformation matrix from the ship's coordinate system to the marker coordinate system; in container port scenarios, infrared cameras are used for observation at night or in foggy weather to improve marker recognition rates.

[0175] (2) Distance or angle measurement observation model.

[0176] UWB observation model: Based on the time difference of arrival algorithm, the distance d between the tugboat and the base station is calculated by the signal transmission time between the tugboat's UWB transceiver and the shore-based UWB base station. Combined with the base station coordinates, the tugboat's position is solved by the trilateration method. In the petrochemical port scenario, frequency hopping technology is used to avoid electromagnetic interference.

[0177] LiDAR observation model: Cluster and segment the LiDAR point cloud to extract the geometric features of the ship or berth (such as edges and corners), and match the feature points through the Iterative Closest Point (ICP) algorithm to obtain the relative pose; In the bulk cargo port scenario, a point cloud denoising algorithm is added to remove invalid point clouds caused by dust scattering.

[0178] (3) Inertial observation and motion compensation.

[0179] The angular velocity and acceleration output by the IMU are processed through pre-integration to obtain the short-term motion trajectory of the tugboat. Combined with the heading angle of the compass, the cumulative error of the IMU is corrected. For visual and ranging observation data at different timestamps, motion compensation is performed through pre-integrated trajectory to eliminate the observation lag caused by the relative motion between the tugboat and the ship, as well as port surges and currents.

[0180] S4: Consistency assessment and confidence generation.

[0181] Residuals and normalized statistics were calculated for observation data from three types of sensors: vision, UWB, and lidar, and hard threshold values ​​were set. (Corresponding to degrees of freedom d=3, significance level α=0.05), soft threshold .

[0182] Container port scenario case: The visual sensor of tugboat 1 is obstructed by the stacked containers, resulting in... If an observation is identified as an outlier, the data is discarded; the system automatically increases the weight of UWB sensors, and the environmental risk factor of UWB is considered. Historical reputation value Consistency score The confidence level was calculated. .

[0183] Bulk cargo port scenario case: The lidar of tugboat 2 is interfered with by dust. Consistency scores are obtained through hard threshold testing and logit mapping. Combined with environmental risk factors Historical reputation value The confidence level was calculated. .

[0184] S5: Adaptive weighting.

[0185] The UWB and lidar observation data that passed the consistency assessment were fused:

[0186] Covariance scaled according to confidence level, UWB observation covariance Confidence level After scaling LiDAR confidence level After scaling .

[0187] The optimization problem is solved by iterative weighted least squares method, which converges after 3 iterations, and the relative pose constraints and covariance after fusion are obtained. In the petrochemical port scenario, the fusion results of explosion-proof sensors are further corrected for errors to ensure that the accuracy meets the requirements of explosion-proof area operations.

[0188] S6: Event-triggered communication and edge selection.

[0189] Container port scenario case: Communication link between tugboat 1 and tugboat 2 is obstructed by container stacking, resulting in high data packet loss rate. This triggers topology reconstruction; tugboat 1 searches its neighborhood nodes and finds the confidence level of tugboat 3. And the link with tugboat 3 Meanwhile, the link quality of the shore-based nodes is good; a temporary topology of "tugboat 1-tugboat 3-shore-based node" is constructed, and information contributions are sent first. The lidar observation side incremental information reduces communication bandwidth usage.

[0190] Petrochemical Port Scenario Case: After a tugboat enters the explosion-proof area, the communication link is switched to an electromagnetic interference-resistant frequency band. Topology reconstruction prioritizes shore-based relay nodes within the explosion-proof area to avoid communication with nodes outside the explosion-proof area, ensuring operational safety.

[0191] S7: Distributed collaborative estimation output.

[0192] The three tugboats send their respective information matrices and information vectors to their neighboring nodes, and perform a weighted average consensus iteration:

[0193] (1) After 5 iterations, the convergence condition is met, and a consistent relative pose estimate is obtained with a pose uncertainty of less than 0.1m;

[0194] (2) When the UWB sensor of tugboat 2 fails, the shore-based RTK blind spot compensation is triggered, and the corrected position and attitude error is maintained within 0.2m, which meets the accuracy requirements of berthing operation;

[0195] (3) The fusion results are synchronized to the port VTS system, which displays the relative position of tugboats and ships and the status of sensors in real time, providing decision-making basis for port regulators.

[0196] This embodiment also provides the following multi-port examples:

[0197] Example of container port operation: A 200,000-ton container ship berths, and three tugboats work together. During the operation, the visual sensor of one tugboat is blocked by the stack of containers. The system automatically discards the visual data, increases the weight of UWB and millimeter-wave radar, and maintains communication through topology reconstruction. Finally, the berthing accuracy of the ship is controlled within 0.3m.

[0198] Example of bulk cargo port: A 150,000-ton iron ore carrier berthed, and two tugboats were equipped with dust-resistant lidar. During the operation, the dust concentration exceeded the standard. The lidar point cloud denoising algorithm effectively eliminated invalid data, and the pose uncertainty after fusion was less than 0.2m, which met the berthing requirements of the bulk carrier.

[0199] Example of a petrochemical port: A 100,000-ton oil tanker berths. The tugboat is equipped with an explosion-proof sensor. During the operation, it enters the explosion-proof area. The system switches to the explosion-proof communication frequency band and activates shore-based RTK to fill the gap. In the end, the oil tanker berths accurately without entering the danger zone, meeting the explosion-proof operation specifications.

[0200] Example 2:

[0201] Embodiment 2 of the present invention provides a ship berthing pose estimation system based on observation confidence self-calibration, comprising:

[0202] The data acquisition module is configured to acquire multi-source observation information from multiple tugboats, assisted vessels, and the target berth, and to perform coordinate unification and time alignment operations on the multi-source observation information.

[0203] The inertial observation module is configured to build a multi-source observation model and use the multi-source observation model to perform inertial observation and corresponding motion compensation on different multi-source observation information.

[0204] The consistency assessment and information fusion module is configured to perform consistency assessment on multi-source observation information based on self-calibrated confidence, and to perform adaptive weighted fusion on multi-source observation information that has passed the consistency assessment. Based on the adaptive weighted fusion result, it executes event-triggered communication and edge selection strategies to obtain the fused pose constraint information.

[0205] The relative pose estimation module is configured to optimize the inertial observation process based on the fused pose constraint information to obtain a consistent relative pose estimate for each tugboat.

[0206] Example 3:

[0207] Embodiment 3 of the present invention provides a computer-readable storage medium storing a computer program adapted for loading by a processor and executing the steps in the ship berthing attitude estimation method based on observation confidence self-calibration as described in Embodiment 1 of the present invention.

[0208] Example 4:

[0209] Embodiment 4 of the present invention provides a computer device, the device comprising:

[0210] A processor, adapted to execute computer programs;

[0211] A computer-readable storage medium storing a computer program, which, when executed by the processor, implements the steps in the ship berthing attitude estimation method based on observation confidence self-calibration as described in Embodiment 1 of the present invention.

[0212] The steps and methods involved in Examples 2, 3 and 4 above correspond to those in Example 1. For specific implementation details, please refer to the relevant description section of Example 1.

[0213] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed in this application can be implemented in electronic hardware or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0214] In the above embodiments, implementation can be achieved, in whole or in part, through software, hardware, firmware, or any combination thereof. When implemented in software, it can be implemented, in whole or in part, as a computer program product. A computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, all or part of the flow or function according to the embodiments of this application is generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in or transmitted through a computer-readable storage medium. The computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via wired or wireless means. The computer-readable storage medium can be any available medium that a computer can access or a data processing device such as a server or data center that integrates one or more available media. The available medium can be a magnetic medium, an optical medium, or a semiconductor medium, etc.

[0215] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A method for estimating ship berthing attitude based on observation confidence self-calibration, characterized in that, Includes the following steps: Acquire multi-source observation information from multiple tugboats, assisted vessels, and target berths, and perform coordinate unification and time alignment operations on the multi-source observation information; A multi-source observation model is constructed, and inertial observation and corresponding motion compensation are performed on different multi-source observation information using the multi-source observation model. Consistency assessment is performed on multi-source observation information based on self-calibrated confidence, and adaptive weighted fusion is performed on the multi-source observation information that passes the consistency assessment. Event-triggered communication and edge selection strategies are executed based on the adaptive weighted fusion results to obtain the fused pose constraint information. The specific steps for conducting a consistency assessment of multi-source observation information based on self-calibrated confidence are as follows: calculate the normalized statistic using the observation residuals and covariance of the observation residuals from the multi-source observation information; conduct a consistency assessment of the normalized statistic based on a two-level threshold consistency test strategy to obtain a consistency score. The consistency score is self-calibrated, and the confidence level is calculated in combination with the port scenario. Specifically, the historical reputation self-calibration model is used to calculate the reputation value of the observation information under different modalities based on the consistency score; the confidence level is calculated by integrating the reputation value, consistency score and port scenario-based environmental risk factors. The specific steps for executing event-triggered communication and edge selection strategies based on the adaptive weighted fusion results are as follows: Set event trigger conditions and edge selection strategy execution conditions; perform topology dynamic reconstruction on the adaptive weighted fusion result based on the event trigger conditions and edge selection strategy execution conditions; The inertial observation process is optimized based on the fused pose constraint information to obtain a consistent relative pose estimate for each tugboat. A port-specific degradation and blind spot compensation strategy is set during the optimization of the inertial observation process based on the fused pose constraint information. When the port-specific degradation and blind spot compensation strategy is triggered, if there are redundant sensors, the observation data of the redundant sensors are automatically switched; if there are no redundant sensors, the blind spot compensation data of the port shore-based facilities is called.

2. The ship berthing attitude estimation method based on observation confidence self-calibration as described in claim 1, characterized in that, The specific steps for performing coordinate unification and time alignment on multi-source observation information include: The data acquisition devices were calibrated separately. Synchronize the time of all data acquisition devices; A four-level coordinate system was established, which was linked to the port surveying benchmark, and all multi-source observation information was unified in coordinates.

3. The ship berthing pose estimation method based on observation confidence self-calibration as described in claim 1, characterized in that, The multi-source observation model includes a visual observation model, a UWB observation model, and a lidar observation model. The visual observation model is used to visually observe the marking images of the hull or berth to obtain the relative pose of the tugboat to the ship. The UWB observation model is used to determine the tugboat position based on the signal transmission time; the lidar observation model is used to cluster and segment the lidar point cloud to obtain the relative pose.

4. A ship berthing attitude estimation system for the ship berthing attitude estimation method based on observation confidence self-calibration as described in any one of claims 1-3, characterized in that, include: The data acquisition module is configured to acquire multi-source observation information from multiple tugboats, assisted vessels, and the target berth, and to perform coordinate unification and time alignment operations on the multi-source observation information. The inertial observation module is configured to build a multi-source observation model and use the multi-source observation model to perform inertial observation and corresponding motion compensation on different multi-source observation information. The consistency assessment and information fusion module is configured to perform consistency assessment on multi-source observation information based on self-calibrated confidence, and to perform adaptive weighted fusion on multi-source observation information that has passed the consistency assessment. Based on the adaptive weighted fusion result, it executes event-triggered communication and edge selection strategies to obtain the fused pose constraint information. The relative pose estimation module is configured to optimize the inertial observation process based on the fused pose constraint information to obtain a consistent relative pose estimate for each tugboat.

5. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program adapted to be loaded by a processor and executed as described in any one of claims 1-3, for the ship berthing attitude estimation method based on observation confidence self-calibration.

6. A computer device, characterized in that, include: A processor, adapted to execute computer programs; A computer-readable storage medium storing a computer program, which, when executed by the processor, implements the ship berthing attitude estimation method based on observation confidence self-calibration as described in any one of claims 1-3.

Images