Shore crane pose estimation fault tolerance method based on multi-sensor fusion

Abstract

The invention discloses a quay crane pose estimation fault tolerance method based on multi-sensor fusion, and belongs to the technical field of port automation equipment. According to the invention, when a quay crane is disconnected due to a network fault, the system compares and analyzes the pre-registered quay crane ID and quay crane placement sequence information with real-time received data, and automatically detects the pose disconnection condition. When a missing quay crane position is detected, a system immediately reports an error and records disconnection information, and different intelligent inference strategies are adopted based on the position sequence of the quay cranes in an area: for the quay crane in the middle position, an intermediate value of front and back adjacent quay cranes is used as an initial pose; for the quay cranes at the edge positions, the approximate poses are inferred based on the positions and overall trends of the adjacent quay cranes. According to the method, a pose estimation algorithm based on point cloud matching is adopted, high-precision relative pose estimation is realized through multi-sensor fusion of the laser radar and the visual camera, and the fault-tolerant capability and operation continuity of a port automation system are remarkably improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of port automation equipment, in particular to a quay crane (QC) pose estimation fault-tolerant method, which is used to infer the fault-tolerant method of the pose of a disconnected quay crane through multi-sensor fusion technology in the case of network disconnection of the quay crane network, so as to ensure the smooth progress of port automation operation. BACKGROUND

[0002] In port automation operation, as an important loading and unloading equipment, the accurate acquisition of the pose information of the quay crane is crucial for the smooth operation of the entire operation process. The position and attitude information of the quay crane is usually transmitted to the relevant management system (such as FMS, Field Management System) through the network, so as to guide the accurate operation of the automated vehicles and other equipment.

[0003] However, in actual operation, due to network failure, equipment damage, etc., the network of a quay crane may be disconnected. Once the network of the quay crane is disconnected, the quay crane will disappear from the graphical user interface (GUI), resulting in the system being unable to obtain its accurate position information. This will affect: 1. FMS task planning: unable to provide accurate end position for unmanned vehicles 2. Unmanned vehicle (IGV, etc.) positioning system: unable to obtain the position of the disconnected quay crane as a positioning reference In the prior art, when the quay crane is disconnected, the following methods are usually used: - Use historical position information as a temporary reference - Suspend related tasks until the problem is solved - Wait for the network to recover and reacquire the position information - Single sensor (such as laser radar) for pose estimation These methods have the problems of slow response speed, long task interruption time, low system reliability, insufficient pose estimation accuracy, etc. SUMMARY

[0004] The present application overcomes the deficiencies in the prior art and provides a multi-sensor fusion-based quay crane pose estimation fault-tolerant method for accurately estimating the pose of the quay crane in the case of network disconnection.

[0005] The multi-sensor fusion-based quay crane pose estimation fault-tolerant method comprises the following steps: 1) Pre-register the quay crane pose information and establish a quay crane pose database; 2) Real-time receive quay crane pose data and compare and analyze with the registered information; 3) Detect the disconnection of the quay crane pose, and immediately report an error and record when a missing quay crane pose is found; 4) Analyze the possible pose range of the missing quay crane, and predict based on historical data and adjacent quay crane information; 5) Calculate the temporary task end point of the n quay crane using the known pose information of the adjacent n-1 and n+1 quay cranes; 6) When the unmanned vehicle arrives at the n-1 quay crane, scan the n quay crane at the marginal position through multiple sensors, including lidar and camera; 7) Multi-sensor feature fusion: fuse the quay girder features extracted by multiple lidars and cameras to obtain the relative pose of the disconnected quay crane relative to the automated vehicle; 8) Global pose calculation: combine the current frame pose of the automated vehicle to obtain the approximate pose of the disconnected quay crane in the global coordinate system; 9) Precise pose optimization: optimize the accurate pose of the disconnected quay crane in the global coordinate system by comparing the quay point cloud detected in real time by the automated vehicle with the pre-built quay point cloud map of the disconnected quay crane; 10) Report the optimized pose result to the FMS system, and dynamically update the navigation pose of the disconnected n quay crane by the FMS system.

[0006] The specific implementation of steps 5) to 10) is as follows: (1) When the FMS receives the notification of the disconnection of the n quay crane; (2) Determine the position type of the n quay crane based on the quay area sequence, i.e., whether it is in the middle position or the edge position; (3) Use different inference strategies according to the position type: a. If in the middle position: use the middle value of the adjacent quay cranes before and after as the initial pose; b. If in the edge position: infer the approximate pose based on the adjacent quay crane pose and the quay spacing rule, which refers to the average spacing between adjacent quay cranes obtained through historical data statistics. The specific inference method is as follows: if the n quay crane is the first end quay crane, use the pose of the n+1 quay crane minus the average spacing to obtain the inferred pose; if the n quay crane is the last end quay crane, use the pose of the n-1 quay crane plus the average spacing to obtain the inferred pose; (4) The FMS first sends the inferred initial pose as the temporary end point; (5) When the automated vehicle arrives at the n-1 quay crane, scan the n quay crane at the marginal position through lidar and visual sensors to extract the quay features; (6) Obtain the relative pose of the disconnected quay crane relative to the automated vehicle through multi-sensor feature fusion, and then combine the global pose of the automated vehicle at the n-1 quay crane to project and calculate the absolute pose of the n quay crane; (7) Report the estimated pose result to the FMS, and dynamically update the task end point to the estimated n quay crane pose by the FMS.

[0007] The step (7) performs multi-sensor scanning and pose estimation if a new task of the n number of quayside crane positions performing FMS dynamic updating is performed, and the specific steps are as follows: (1) Multi-sensor fusion system: composed of multiple laser radars and multiple camera systems, the multiple laser radars are configured to cooperate with front and rear horizontal lasers and vertical laser radars; the multiple camera systems are left, right, front and rear cameras for full coverage; sensor data fusion is intelligent fusion of laser point cloud and visual features, and time alignment and synchronization of multi-sensor data; (2) First, multi-sensor features are extracted, which are respectively: laser radar feature extraction: quayside vertical structure, beam feature, support column feature; visual feature extraction: quayside identification, color feature, texture feature, edge feature; Secondly, the features are fused: intelligent association and matching of multi-sensor features; Finally, the features are verified: consistency verification of multi-angle features; (3) Pose optimization is performed: first, real-time point cloud and pre-built map matching: accurate matching of current detection point cloud and historical map; then, global pose optimization: global pose calculation based on multi-sensor fusion; again, confidence evaluation: multi-dimensional confidence calculation and verification; finally, dynamic update: dynamic update of real-time pose information; (4) The updated real-time pose information is verified for rationality, and the specific steps are as follows: a. Initial pose comparative analysis: comparing and analyzing the pose calculated by multi-sensor fusion with the initial pose inferred by FMS; b. Deviation calculation: calculating the position deviation and angle deviation between the calculated pose and the inferred initial pose; c. Rationality judgment: based on the preset deviation threshold, it is judged whether the calculation result is reasonable; the verified disconnected quayside crane pose real-time reports the estimated result to FMS: dynamically updates the task end point pose and ensures the continuity of the unmanned vehicle task.

[0008] The core innovation of the present application is that the features of the quayside crane extracted by the multi-sensor (multiple laser radars and cameras) are fused, the relative position of the disconnected quayside crane relative to the unmanned vehicle is obtained, the approximate position of the disconnected quayside crane in the global coordinate system is obtained combined with the current frame pose of the unmanned vehicle, and finally the accurate position of the disconnected quayside crane in the global coordinate system is matched and optimized through the quayside crane point cloud detected by the unmanned vehicle and the pre-built quayside crane point cloud map of the disconnected quayside crane.

[0009] The present application has the following beneficial effects: 1. Improve system fault tolerance, reduce the impact of network failure or equipment failure on operation; 2. Real-time estimation of disconnected shore-to-bridge position ensures continuous task continuity; 3. Multi-sensor fusion significantly improves pose estimation accuracy and reliability; 4. Reduces system maintenance costs and improves port operation efficiency.

[0010] The technical innovations of the present application are as follows: 1. The first multi-sensor fusion-based disconnected shore-to-bridge pose estimation method is proposed; 2. Innovative design of multi-laser radar and camera cooperative working architecture; 3. Unique implementation of intelligent fusion algorithm for laser point cloud and visual features; 4. System integration of real-time point cloud and pre-built map precise matching optimization; 5. Architecture innovation of multi-sensor time synchronization and data alignment mechanism; 6. Algorithm innovation of multi-dimensional confidence evaluation and verification system; 7. Verification mechanism innovation based on FMS inference position and other shore-to-bridge pose rationality verification mechanism.

[0011] 1. High reliability - Through multi-sensor fusion, reduce the impact of single point failure; - Multiple verification mechanisms ensure the accuracy of the estimation results; - Abnormal handling and degradation strategy improves system robustness; - Multi-sensor cross-verification improves reliability.

[0012] 2. High precision - Multi-sensor fusion improves feature extraction accuracy; - Real-time point cloud and pre-built map precise matching; - Position accuracy: ±0.05m; - Angle accuracy: ±0.2°; - Confidence threshold: >0.9.

[0013] 3. Real-time - Disconnected detection time <0.5s; - Pose estimation time <3s; - Task update delay <5s; - Multi-sensor parallel processing improves response speed.

[0014] 4. High degree of automation - Fully automatic detection, estimation and reporting process; - Intelligent retry and abnormal handling mechanism; - Multi-sensor automatic fusion and verification; - Fault-tolerant recovery without human intervention.

[0015] 5. Strong compatibility - Seamless integration with existing FMS and unmanned vehicle systems; - Support for multiple sensor configurations and extensions; - Scalable algorithm framework and architecture; - Modular design for easy maintenance and upgrade.

[0016] Experimental verification and effect data The invention has been tested for 3 months in a large container port, verifying the high precision and fault tolerance of the system. The test environment includes 10 quayside cranes (QC01-QC10) with an average distance of 40m, and 30 quayside crane disconnection scenarios are simulated during the test period.

[0017] 1. Pose estimation accuracy verification By comparing the estimated pose of the quayside crane with the true pose after the quayside crane is reconnected, the following statistics are obtained: (1) Position estimation accuracy: - Average position error: 0.032m - Position error standard deviation: 0.018m - Position error within 95% confidence interval: <0.05m - Maximum position error: 0.087m (2) Angle estimation accuracy: - Average angle error: 0.15° - Angle error standard deviation: 0.08° - Angle error within 95% confidence interval: <0.2° - Maximum angle error: 0.31° (3) Comparative experiment: Compared with the existing single laser radar method, the multi-sensor fusion method of the invention improves the position accuracy by 67% and the angle accuracy by 58%.

[0018] 2. System fault tolerance verification (1) Disconnection detection performance: - Average disconnection detection time: 0.28 seconds - Detection success rate: 100% (30 / 30 times) - No false positives and false negatives (2) Pose estimation success rate: - Initial estimation success rate: 93.3% (28 / 30 times) - Success rate after retry: 100% (30 / 30 times) - Average estimation time: 2.1 seconds (3) Task continuity guarantee: - After adopting the invention, the task interruption time in the disconnected shore bridge task is shortened from an average of 15 minutes to 8 seconds - The task success completion rate is improved from 0% when disconnected to 96.7% - The waiting time of the automated vehicle is reduced by 99.1% 3. Confidence evaluation verification Statistical confidence distribution in 30 tests: - Confidence > 0.9: 23 times (76.7%), average error of pose estimation 0.025m - Confidence 0.7-0.9: 5 times (16.7%), average error of pose estimation 0.048m - Confidence < 0.7: 2 times (6.7%), successful after triggering re-scanning The high correlation between confidence evaluation and actual accuracy is verified.

[0019] 4. Different scene adaptability verification (1) Different position types: - Middle position shore bridge (QC02-QC09): average error 0.028m, success rate 100% - Edge position shore bridge (QC01, QC10): average error 0.042m, success rate 90% (2) Different environmental conditions: - Daylight with sufficient light: average error 0.030m - Night light environment: average error 0.035m - Rainy day with reduced visibility: average error 0.041m (still meets the requirement of <0.05m) 5. System benefit evaluation After 3 months of actual application, the system brings significant benefits: (1) Operation efficiency improvement: - The number of operation interruptions caused by disconnected shore bridges is reduced by 95% (2) Economic benefits: - Reduce about 80% of manual intervention cost (3) System reliability: - System stable operation rate 99.7% - No safety accidents occurred Through the above experimental data verification, the application indeed realizes high-precision relative pose estimation, significantly improves the fault tolerance and operation continuity of the port automation system, and has important practical value and popularization prospect. BRIEF DESCRIPTION OF DRAWINGS

[0020] Figure 1 : is a complete flowchart from system monitoring to detecting pose disconnection, then recording disconnection information and notifying the FMS system; Figure 2 : is a full flowchart of the FMS receiving disconnection notification, then estimating the disconnected shore crane pose through multi-sensor fusion, and then updating the navigation task; Figure 3 : is a flowchart of the unmanned vehicle reaching the n-1 QC to finally project the rough global coordinates of the shore crane; Figure 4 : is a flowchart of reporting the FMS after obtaining the rough global coordinates of the disconnected shore crane, and updating the task to make the unmanned vehicle continue to execute to the estimated position; Figure 5 : is a matching effect diagram of the real-time laser of the disconnected shore crane and the pre-mapping shore crane point cloud; Figure 6 : is a multi-sensor fusion system architecture block diagram of the application. DETAILED DESCRIPTION

[0021] The application will be further described below in conjunction with the drawings.

[0022] As shown in Figures 1-4 , the shore crane pose estimation fault-tolerant method based on multi-sensor fusion includes the following steps: 1) Pre-registering shore crane pose information to establish a shore crane pose database; 2) Real-time receiving of shore crane pose data and comparison analysis with registered information; 3) Detecting the disconnection of the shore crane pose, and immediately reporting an error and recording when a missing shore crane pose is found; 4) Analyzing the possible pose range of the missing shore crane, and predicting based on historical data and adjacent shore crane information; 5) Using the known pose information of the adjacent n-1 and n+1 shore cranes to calculate the temporary task end point of the n shore crane; 6) When the unmanned vehicle reaches the n-1 shore crane, the multi-sensor scans the n shore crane at the marginal position, and the sensors are laser radar and camera; 7) Multi-sensor feature fusion: fusing the shore beam features extracted by multiple laser radars and cameras to obtain the relative pose of the disconnected shore crane relative to the automated vehicle; 8) Global pose calculation: combining the current frame pose of the automated vehicle to obtain the approximate pose of the disconnected shore crane in the global coordinate system; 9) Precise pose optimization: through automatic real-time detection of the disconnected quay crane point cloud and loading of the pre-built quay crane point cloud map of the disconnected quay crane, the accurate pose of the disconnected quay crane in the global coordinate system is optimized; 10) Report the optimized pose result to the FMS system, and the FMS system dynamically updates the navigation pose of the n-th quay crane.

[0023] The specific implementation of steps 5) to 10) is as follows: (1) When the FMS receives the n-th quay crane disconnection notification; (2) Determine the position type of the n-th quay crane based on the quay crane area sequence, that is, whether it is a middle position or an edge position; (3) Different inference strategies are adopted according to the position type: a. If in the middle position: use the middle value of the adjacent quay cranes before and after as the initial pose; b. If in the edge position: infer the approximate pose based on the adjacent quay crane pose and the quay crane spacing rule, which refers to the average spacing between adjacent quay cranes obtained through historical data statistics. The specific inference method is: if the n-th quay crane is the first end quay crane, use the n+1-th quay crane pose minus the average spacing to obtain the inferred pose; if the n-th quay crane is the last end quay crane, use the n-1-th quay crane pose plus the average spacing to obtain the inferred pose; (4) The FMS first sends the inferred initial pose as a temporary terminal point; (5) When the automated vehicle arrives at the n-1-th quay crane, scan the n-th quay crane at the edge position through laser radar and vision sensor to extract the quay crane features; (6) Obtain the relative pose of the disconnected quay crane relative to the automated vehicle through multi-sensor feature fusion, and combine the global pose of the automated vehicle under the n-1-th quay crane to project and calculate the absolute pose of the n-th quay crane; (7) Report the estimated pose result to the FMS, and the FMS dynamically updates the task terminal point to the estimated n-th quay crane pose.

[0024] If the n-th quay crane position is updated by the FMS in step (7), multi-sensor scanning and pose estimation are performed, which are specifically performed according to the following steps: (1) Multi-sensor fusion system: composed of multiple laser radars and multiple camera systems, the multiple laser radars are configured to work cooperatively with front and rear horizontal lasers and vertical laser radars; the multiple camera system is a full-range coverage of left, right, front and rear cameras; sensor data fusion is intelligent fusion of laser point cloud and vision features, and time alignment and synchronization of multi-sensor data; (2) Firstly, multi-sensor features are extracted, which are: laser radar feature extraction: shore bridge vertical structure, beam feature, support column feature; vision feature extraction: shore bridge identification, color feature, texture feature, edge feature; Secondly, the features are fused: intelligent association and matching of multi-sensor features; Finally, the features are verified: consistency verification of multi-angle features; (3) Pose optimization: firstly, real-time point cloud and pre-built map matching: accurate matching of current detection point cloud and historical map; then global pose optimization: global pose calculation based on multi-sensor fusion; again, confidence assessment: multi-dimensional confidence calculation and verification; finally, dynamic update: dynamic update of real-time pose information; (4) Reasonable verification of the above updated real-time pose information, the specific steps are as follows: a. Initial pose comparative analysis: comparing the pose calculated by multi-sensor fusion with the initial pose inferred by FMS; b. Deviation calculation: calculating the position deviation and angle deviation between the calculated pose and the inferred initial pose; c. Reasonableness judgment: based on the preset deviation threshold, judge whether the calculation result is reasonable; the verified disconnected shore bridge pose real-time reports the estimated result to FMS: dynamically updates the task end pose and ensures the continuity of the unmanned vehicle task.

[0025] Among them: Figure 1 is the complete flowchart from system monitoring to detecting pose disconnection, then recording disconnection information and notifying FMS system. Specifically, it includes: after the system starts, it enters the real-time monitoring state, receives the shore bridge pose data, judges whether there is a shore bridge disconnection for the first time (by comparing the registration information with the real-time data), if there is no disconnection, continue to monitor; if disconnection is detected, enter the second confirmation judgment (to prevent network jitter misjudgment), after two confirmations, record the disconnection information and notify the FMS system, and update the system state. The two disconnection judgment mechanisms ensure the accuracy of detection, the first judgment is preliminary detection, and the second judgment is a delay confirmation (delay 0.5 seconds), which avoids false positives caused by transient network fluctuations.

[0026] Figure 2is the full flow chart of estimating the disconnected shore crane pose by multi-sensor fusion after receiving the disconnection notification by FMS, and then updating the navigation task. The flow includes: FMS receives disconnection notification → judges the shore crane position type (middle / edge) → infers the initial pose → sends the temporary end point to the vehicle → the vehicle arrives at the n-1 shore crane → starts the multi-sensor scanning and pose estimation module → obtains the accurate pose → reports to FMS → updates the task. Among them, "multi-sensor scanning and pose estimation" includes data acquisition, feature extraction, feature fusion, relative pose calculation and other steps of laser radar and camera, which is a general description of steps 7) - 9).

[0027] Figure 3 is the flow chart of finally projecting the rough shore crane global coordinates after the unmanned vehicle arrives at the n-1 QC. The detailed process is: the vehicle arrives at the n-1 shore crane edge position → multi-sensor data acquisition → extract shore crane features (cross beam, support column, etc.) → calculate the relative pose (i.e. the relative pose of the disconnected shore crane relative to the vehicle, including relative distance and relative angle) → obtain the current global pose of the vehicle → calculate the rough global pose of the disconnected shore crane through coordinate transformation projection. "Calculate the relative pose" specifically refers to: through the multi-sensor fusion extracted shore crane features, calculate the position and orientation of the disconnected shore crane in the vehicle coordinate system, and get the relative pose vector (dx, dy, dθ).

[0028] Figure 4 is the flow chart of reporting FMS after obtaining the rough disconnected shore crane global coordinates, and updating the task to make the unmanned vehicle continue to execute to the estimated position. The detailed process includes: obtaining the rough pose → point cloud preprocessing (filtering, denoising, downsampling, etc. on the real-time collected shore crane point cloud) → loading the pre-built map → multi-algorithm fusion (using ICP, NDT and other point cloud registration algorithms for parallel calculation, and taking the optimal result) → iterative optimization → generating accurate pose (obtaining the final accurate pose through optimization algorithm) → adjacent shore crane pose consistency verification (checking whether the estimated pose and the adjacent shore crane are reasonable in distance and orientation, including calculating whether the distance from the adjacent shore crane conforms to the average distance rule, checking whether the orientation angle is close to the adjacent shore crane, etc.) → judging whether the verification passes → if it passes, reporting to FMS and updating the task → if it does not pass, reloading the map or triggering re-scanning (adjusting the vehicle position or replacing the sensor view to re-collect data).

[0029] As shown in Figure 5 , the disconnected shore crane real-time laser and pre-built shore crane point cloud matching effect diagram, if the two kinds of point clouds coincide, it means that the real-time estimated shore crane pose is accurate; if the two kinds of point clouds do not coincide, it means that the real-time estimated shore crane pose is not accurate.

[0030] The application includes two core application scenarios: one is FMS task end point dynamic update, when the disconnected shore bridge pose needs to be taken as the task end point, a temporary end point is set first, and then the real pose is estimated by automatic vehicle scanning and dynamically updated; the other is real-time pose estimation of the positioning system, which provides continuous reference information for the positioning system.

[0031] As Figure 6 shown, the application includes: pre-built database, real-time monitoring and disconnection monitoring, missing pose prediction and temporary task planning, multi-sensor scanning and feature fusion, and global pose calculation and optimization.

[0032] Among them: I, pre-built database: 1. Register the id information of all shore bridges and the relative order of the shore bridge placement positions; 2. Pre-construct a point cloud map corresponding to each shore bridge id.

[0033] II. Real-time monitoring and disconnection monitoring As Figure 1 shown, the system compares and analyzes the pre-registered shore bridge pose information with the real-time received shore bridge pose data to automatically detect whether there is a shore bridge pose disconnection. The specific process is: (1) After the system starts, it enters a real-time monitoring state and periodically receives the pose data of all shore bridges; (2) First disconnection judgment: compare the registered shore bridge list with the real-time received shore bridge data, if some shore bridge data is missing, mark it as suspected disconnection; (3) Second disconnection confirmation: to avoid network jitter misjudgment, check again after a delay of 0.5 seconds, if the shore bridge data is still missing, confirm disconnection; The necessity of the two judgment mechanisms lies in: in network communication, there may be transient packet loss or delay, single judgment is easy to misreport, through delay twice confirmation, the real disconnection and network jitter can be effectively distinguished, and the detection accuracy is ensured. Experimental data shows that the two judgment mechanisms reduce the false positive rate from 12% to 0%.

[0034] When the missing shore bridge pose is detected, the system will: 1. Immediately report an error and record the disconnection information (including shore bridge ID, disconnection time, last known pose, etc.); 2. Analyze the possible pose range of the missing shore bridge based on the regional order of the shore bridge; 3. Use different inference strategies according to the position of the shore bridge in the region; 4. Notify the FMS system to start intelligent reasoning and pose estimation process.

[0035] III. Missing pose prediction and temporary task planning As Figure 2As shown, when FMS needs the pose of disconnected shore bridge n as the mission endpoint, the complete process is as follows: 1. The system detected a disconnection in the pose of quay crane number n, immediately reported an error, and recorded it; 2. Determine the location type (middle or edge) of quay bridge n based on the quay bridge area sequence. 3. Employ different inference strategies based on location type: - Midpoint position: Use the midpoint between the two adjacent quay cranes as the initial pose; - Edge position: The approximate pose is inferred based on the poses of adjacent quaybridges and the pattern of quaybridge spacing. The pattern of quaybridge spacing refers to the average spacing between adjacent quaybridges obtained through historical data statistics. The specific inference method is as follows: if quaybridge n is the first quaybridge, the inferred pose is obtained by subtracting the average spacing from the pose of quaybridge n+1; if quaybridge n is the last quaybridge, the inferred pose is obtained by adding the average spacing to the pose of quaybridge n-1. 4. FMS first sends the inferred initial pose as a temporary endpoint; 5. When the automated vehicle arrives at quay crane n-1, it activates the "multi-sensor scanning and pose estimation" module at the edge location. This module includes: (a) Multi-sensor data acquisition: LiDAR acquires point cloud data, and camera acquires image data; (b) Feature extraction: Extract features such as the quay bridge beams and support columns from the point cloud and images; (c) Feature fusion: Fusing features extracted from multiple sensors to improve feature localization accuracy; (d) Relative pose calculation: The pose of the disconnected quay crane relative to the vehicle is calculated based on the fused features; like Figure 3 As shown, the specific process of "calculating relative pose" is as follows: Step 1: Calculate the coordinates of the quay crane features (such as the center point of the crossbeam) extracted by multi-sensor fusion in the vehicle coordinate system; Step 2: Calculate the relative distances dx and dy and the relative angle dθ based on the geometric relationships; Step 3: Obtain the relative pose vector (dx, dy, dθ); Step 4: Obtain the vehicle's current global pose (x_v, y_v, θ_v); Step 5: Calculate by projection through coordinate transformation: x_qc = x_v + dx × cos(θ_v) - dy × sin(θ_v) y_qc = y_v + dx × sin(θ_v) + dy × cos(θ_v) θ_qc = θ_v + dθ Step 6: Obtain the rough pose of the disconnected quay crane in the global coordinate system (x_qc, y_qc, θ_qc); 6. Obtain the relative pose of the disconnected quay crane with respect to the automated vehicle through multi-sensor feature fusion, and combine the global pose of the automated vehicle under the n-1 quay crane to project and calculate the absolute pose of the n quay crane; 7. Report the estimated pose result to the FMS, and dynamically update the task end point to the estimated n quay crane pose.

[0036] IV. Multi-sensor scanning and feature fusion The unmanned vehicle triggers the online estimation function of the disconnected quay crane pose at the edge position of the n-1 quay crane. Through the quay features extracted by multiple sensors (multiple laser radars and cameras), the quay features extracted by multiple sensors are fused to obtain the relative position of the disconnected quay crane with respect to the automated vehicle. Combined with the current frame pose of the automated vehicle, the approximate position of the disconnected quay crane in the global coordinate system is obtained. Finally, through the quay point cloud detected by the automated vehicle in real time and the pre-built quay point cloud map of the disconnected quay crane, the accurate position of the disconnected quay crane in the global coordinate system is optimized.

[0037] V. Global pose calculation and optimization As shown in Figure 4 After obtaining the relative pose of the disconnected quay crane with respect to the automated vehicle, the detailed process of precise pose optimization is as follows: 1. Point cloud preprocessing: preprocess the real-time collected disconnected quay crane point cloud, including: (a) Distance filtering: remove noise points with a distance > 50m or < 0.5m; (b) Statistical filtering: remove outliers and retain valid point clouds; (c) Voxel filtering: downsample using a 0.05m x 0.05m x 0.05m voxel grid to reduce computational load; (d) Ground removal: remove ground points by RANSAC plane fitting; 2. Load pre-built map: load the pre-built point cloud map of the disconnected quay crane from the database according to the quay crane ID; 3. Multi-algorithm fusion registration: use multiple point cloud registration algorithms in parallel to improve accuracy and robustness: (a) ICP algorithm: iterative closest point algorithm, suitable for cases where the initial pose is relatively accurate; (b) NDT algorithm: normal distribution transform algorithm, suitable for complex environments; (c) Feature matching algorithm: feature point matching algorithm based on quay features; The multi-algorithm fusion registration refers to parallel computing of multiple point cloud registration algorithms (ICP, NDT, feature matching algorithm), and the specific implementation method is as follows: (1) Parallel execution of multiple registration algorithms: - ICP algorithm: iterative closest point registration of real-time point cloud and pre-built map to obtain registration result Result_ICP, including transformation matrix T_ICP and registration error Error_ICP; - NDT algorithm: normal distribution transformation registration of real-time point cloud and pre-built map to obtain registration result Result_NDT, including transformation matrix T_NDT and registration error Error_NDT; - Feature matching algorithm: matching based on shore bridge feature points to obtain registration result Result_Feature, including transformation matrix T_Feature and registration error Error_Feature; (2) Evaluate the registration quality of each algorithm: Calculate the comprehensive score Score_i of each algorithm: Score_i = w1 x (1 / Error_i) + w2 x Confidence_i, where Error_i is the registration error, Confidence_i is the confidence, and w1 and w2 are weight coefficients (w1 = 0.6, w2 = 0.4); - For ICP algorithm, Error_ICP is the root mean square error (RMSE) of point pair distance; - For NDT algorithm, Error_NDT is the matching error of transformed point cloud and NDT grid; - For the feature matching algorithm, Error_Feature is the geometric error of feature point matching; (3) Select the optimal result: - Compare Score_ICP, Score_NDT, Score_Feature, and select the result with the highest score as the final registration result; - If the difference between the highest score and the second highest score is less than a threshold (such as 10%), then use weighted fusion: T_final = a x T_best + (1-a) x T_second, where a is determined according to the score ratio; Through the above method, the fusion registration of multiple algorithms is realized; 4. Iterative optimization: iterative optimization of the initial registration result obtained by multi-algorithm fusion registration, including the following steps: (a) Initialization: take the initial pose obtained by multi-algorithm fusion registration as the initial value of iterative optimization; (b) Nearest neighbor matching: establish the nearest neighbor point pair relationship between real-time point cloud S and pre-built map M; (c) Compute rigid transformation: solve rotation increment ΔR and translation increment Δt by minimizing point-to-point or point-to-plane error; (d) Update pose: T_new = ΔT · T_old, where T_old is the current pose transformation matrix, T_new is the updated pose transformation matrix, and ΔT is the transformation matrix composed of ΔR and Δt; Wherein: the source of the current pose transformation matrix "T_old" is as follows: (1) In the initial stage of iterative optimization, i.e. initialization of step a, T_old is initialized as the initial pose transformation matrix obtained by multi-algorithm fusion registration; that is: T_old = T_initial, where T_initial is the initial pose transformation matrix obtained by the multi-algorithm fusion registration step; (2) In the iteration process, T_old represents the pose transformation matrix of the current iteration step; in the first iteration, T_old is the initial pose transformation matrix T_initial; in subsequent iterations, T_old is the updated pose transformation matrix T_new of the last iteration; (3) The specific updating process is: the updated pose transformation matrix T_new is calculated by the formula T_new = ΔT · T_old; at the beginning of the next iteration, T_new is assigned to T_old as the new "current pose transformation matrix" for continuous iteration; Therefore, the obtaining process of the "current pose transformation matrix" T_old is: first, the initial pose transformation matrix obtained by multi-algorithm fusion registration is taken as the initial value of T_old, and then in the iterative optimization process, T_new is updated and assigned to T_old to realize the iterative optimization of the pose transformation matrix; (e) Calculate matching error: calculate the point cloud matching error under the updated pose; (f) Convergence determination: if the pose increment (including translation increment and rotation increment) and the matching error are reduced below the preset threshold, stop iteration, otherwise return to step (b) for continuous iteration.

[0038] To improve the robustness and accuracy of iterative optimization, the following improvement measures are adopted: - Point-to-plane ICP: not only the point-to-point distance but also the point-to-local plane distance is considered to improve the registration accuracy; - Weighted ICP: different weights are given according to the importance of features, the beam feature weight is set to 1.5, and the weight of other features is 1.0; - Multi-resolution ICP: first use down-sampled point cloud for fast convergence, and then use fine point cloud for accurate optimization to improve the calculation efficiency; Through iterative optimization, the optimized pose transformation matrix T_optimized is obtained, and then the optimized pose (x_iter, y_iter, θ_iter) is obtained. 5. Generating accurate pose: Nonlinear optimization of registration results by optimization algorithm (such as Levenberg-Marquardt) to obtain final accurate pose (x_opt, y_opt, θ_opt); 6. Verification of adjacent quay crane pose consistency: Check the rationality of the estimated pose: (a) Calculate the distance from the adjacent quay crane and check whether it meets the average distance rule; (b) Check if the orientation angle is close to the adjacent quay crane (error < 15°); (c) Calculate the deviation from the initial pose inferred by FMS, and check whether it is within a reasonable range (position deviation < 5m, angle deviation < 10°); 7. Judge the verification result: (a) If the verification is passed, report the accurate pose to FMS, FMS updates the task end point, and the vehicle continues to execute the task; (b) If the verification fails, trigger abnormal processing: - Reload the map: Check if the wrong quay crane map is loaded, and load the correct map after reconfirming the quay crane ID; - Trigger re-scanning: Adjust the position or angle of the vehicle, and re-collect point cloud data from a different perspective, and re-register and verify; - If multiple retries still fail (> 3 times), report the exception to FMS, and use a degradation strategy (such as the initial pose inferred by FMS); Through the above process, the finally generated pose is both accurate and reliable. Experimental data shows that the multi-algorithm fusion registration improves the accuracy by 40% compared to a single algorithm, and the adjacent quay crane pose consistency verification effectively reduces the abnormal results by 85%.

[0039] The application adopts a pose estimation algorithm based on point cloud matching, realizes high-precision relative pose estimation through multi-sensor fusion of laser radar and visual camera. At the same time, the system has the characteristics of automatic detection, intelligent estimation and real-time reporting, which significantly improves the fault tolerance and operation continuity of the port automation system.

[0040] Six, specific implementation method of key technology In order for those skilled in the art to implement the present application, the specific implementation method of the key technology steps is described in detail as follows: (Zero) Specific method of missing quay crane pose initial inference When the n-th quay crane is detected to be disconnected, an initial pose inference needs to be made based on its position type to provide an initial value for subsequent accurate estimation.

[0041] 1. Quay crane position type determination Suppose there are M quay cranes in the port area, numbered QC_1, QC_2,..., QC_M, arranged in order along the shoreline.

[0042] For the disconnected n-th quay crane, determine its position type: (a) Middle position: If 1 < n < M, there are other quay cranes before and after this quay crane. (b) First edge position: If n = 1, the quay crane is at the front end. (c) Last edge position: If n = M, the quay crane is at the end.

[0043] 2. Quay crane spacing rule statistics The "quay crane spacing rule" specifically refers to: (a) Historical data collection: During normal system operation, continuously record the pose information of all online quay cranes and store them in the database. (b) Spacing calculation: For two adjacent quay cranes QC_i and QC_{i+1}, calculate the distance between them. (c) Average spacing statistics: For all adjacent quay crane spacings in the historical data, calculate the average value. (d) Standard deviation calculation, used to assess the stability of the spacing. (e) Spacing rule update: The system regularly updates the average spacing d_avg to adapt to changes in quay crane layout, such as every day.

[0044] Typical values: For large container ports, the average spacing of quay cranes d_avg is usually 30m~50m, and the standard deviation σ_d < 5m.

[0045] 3. Inference method for middle-position quay cranes When the n-th quay crane is in the middle position (1 < n < M): (a) Get the pose of adjacent quay cranes: Previous quay crane: Pos_{n-1} = (x_{n-1}, y_{n-1}, θ_{n-1}) Pos_{n-1} represents the pose of the n-1th quay crane, including its x value, y value, and angle in the map coordinate system. Next quay crane: Pos_{n+1} = (x_{n+1}, y_{n+1}, θ_{n+1}) Pos_{n+1} represents the pose of the n+1st quay crane, including its x value, y value, and angle in the map coordinate system; (b) Position inference employs linear interpolation (midpoint): x_n_init = (x_{n-1} + x_{n+1}) / 2 x_n_init represents the x-coordinate of the n-th quay crane in the map coordinate system; y_n_init = (y_{n-1} + y_{n+1}) / 2 y_n_init represents the y-coordinate of the n-th quay crane in the map coordinate system; (c) Orientation inference employs the average value: θ_n_init = (θ_{n-1} + θ_{n+1}) / 2 θ_n_init represents the angle coordinate of the n-th quay crane in the map coordinate system; Note the handling of angle crossing ±π: If |θ_{n+1} - θ_{n-1}|>π, first perform angle normalization processing; (d) Obtain the initial inferred pose: Pose_n_init = (x_n_init, y_n_init, θ_n_init) Pose_n_init represents the predicted pose of the n-th quay crane in the map coordinate system; (e) Inference accuracy evaluation: Expected position error: Δx ≈ |x_{n+1} - x_{n-1}| / 4 Expected position error analysis: Since the position inference employs linear interpolation (taking the midpoint of the positions of the two adjacent quay cranes), when the three quay cranes are arranged at equal intervals, let the interval between adjacent quay cranes be d, then the distance from the previous quay crane to the next quay crane is |x_{n+1} - x_{n-1}| = 2d. The maximum error of linear interpolation is about half the interval, as follows: When the n-th quay crane is in the middle position, the linear interpolation method is used to infer its position: x_n_init = (x_{n-1} + x_{n+1}) / 2 Assuming that the three quay cranes are arranged at equal intervals, with an interval of d, then: - The distance from the previous quay crane to the next quay crane is |x_{n+1} - x_{n-1}| = 2d - Linear interpolation takes the midpoint between the n-1 and n+1 quays In the worst case, if the n-th quay is located at the maximum distance from the midpoint, which is d / 2 (i.e. half the distance). This is because: - If the n-th quay is located closer to the n-1 quay, the maximum deviation is d / 2 - If the n-th quay is located closer to the n+1 quay, the maximum deviation is also d / 2 - Therefore, the maximum error of linear interpolation is approximately half the distance, i.e. Δx_max ≈ d / 2 Since |x_{n+1} - x_{n-1}| = 2d, we have: Δx_max ≈ d / 2 = (2d) / 4 = |x_{n+1} - x_{n-1}| / 4 Therefore, the expected position error: Δx ≈ |x_{n+1} - x_{n-1}| / 4, thus divided by 4;

[0046] For regularly spaced quays, this error is typically <10m.

[0047] 4. Inferred method for the position of the first edge quay, where the first edge: the starting position of the quay arrangement sequence, i.e. the quay numbered 1 (QC_1), is located at the very front of the entire quay arrangement sequence; When the n=1 quay is at the first edge: (a) Get the pose of the adjacent quays: Next quay: Pos_2 = (x_2, y_2, θ_2) (b) Calculate the direction vector: If there is a third quay online (QC_3), the direction trend of the quay arrangement can be calculated: Direction vector: v = (x_3 - x_2, y_3 - y_2) Normalize: v_norm = v / ||v|| (c) Position inference method: Case 1: If QC_3 is online, infer based on the direction trend: x_1_init = x_2 - d_avg × v_norm.x y_1_init = y_2 - d_avg × v_norm.y Where the meaning of "v_norm.x, v_norm.y" is as follows: v_norm is the normalized direction vector, and v_norm.x and v_norm.y represent the components of the vector in the x and y axis directions, respectively. The specific description is as follows: (1) Calculation of direction vector v: v = (x_3 - x_2, y_3 - y_2) or v = (x_{M-1} - x_{M-2}, y_{M-1} - y_{M-2}) This is a two-dimensional vector representing the direction of the quay crane arrangement.

[0048] (2) Normalization processing: v_norm = v / ||v|| Where ||v|| is the length of vector v (Euclidean distance): ||v|| = sqrt((v.x)² + (v.y)²) = sqrt((x_3 - x_2)² + (y_3 - y_2)²) After normalization, the length of v_norm is 1, i.e., ||v_norm|| = 1.

[0049] (3) Component representation: v_norm.x represents the component (scalar value) of the normalized vector in the x axis direction; v_norm.y represents the component (scalar value) of the normalized vector in the y axis direction; Satisfying the relationship: (v_norm.x)² + (v_norm.y)² = 1 (4) Application in position inference: x_1_init = x_2 - d_avg × v_norm.x y_1_init = y_2 - d_avg × v_norm.y Indicates that along the normalized direction vector, moving d_avg distance in the opposite direction from the QC_2 position, the inferred position of QC_1 is obtained.

[0050] Case 2: If only QC_2 is online, use the average interval and the orientation of QC_2: x_1_init = x_2 - d_avg × cos(θ_2) y_1_init = y_2 - d_avg × sin(θ_2) (d) Orientation inference: θ_1_init = θ_2 (Assuming consistent orientation of adjacent quay cranes) (e) Obtain the initial inferred pose: Pose_1_init = (x_1_init, y_1_init, θ_1_init) (f) Inferred accuracy assessment: Expected position error: Δx ≈ σ_d (standard deviation) Where the derivation of "Δx ≈ σ_d (standard deviation)" is as follows: For the edge-positioned quay crane (either the first or the last), the position inference is based on the adjacent quay crane pose and the average spacing d_avg. The inference error mainly comes from the statistical fluctuation of the quay crane spacing.

[0051] (1) Statistical properties of spacing: Through historical data statistics, the spacing d between adjacent quay cranes follows a normal distribution with a mean of d_avg and a standard deviation of σ_d.

[0052] That is: d ~ N(d_avg, σ_d²) (2) Position inference method: For the first quay crane: x_1_init = x_2 - d_avg × v_norm.x For the last quay crane: x_M_init = x_{M-1} + d_avg × v_norm.y (3) Error analysis: The deviation of the actual spacing d from the average spacing d_avg is: Δd = d - d_avg Since d follows a normal distribution N(d_avg, σ_d²), Δd follows a normal distribution N(0, σ_d²) The position inference error mainly comes from the spacing deviation, so: Δx ≈ |Δd| × |v_norm.x| ≈ |Δd| Since the standard deviation of Δd is σ_d, at a 68% confidence level, |Δd| ≈ σ_d Therefore, the expected position error: Δx ≈ σ_d (4) Practical application: For regularly arranged quay cranes, σ_d is usually <5m, so the inference error is usually <5m, meeting the accuracy requirements of temporary task endpoints.

[0053] 5. Inferred method for the end edge-positioned quay crane, where the end: the end position of the quay crane arrangement sequence, i.e. the quay crane numbered M (QC_M), is located at the end of the entire quay crane arrangement sequence; When n = M, the last quay crane is online: (a) Get the pose of the adjacent quay crane: Previous quay crane: Pos_{M-1} = (x_{M-1}, y_{M-1}, θ_{M-1}) (b) Compute the direction vector: If there is a third last quay crane online (QC_{M-2}), compute the direction trend: Direction vector: v = (x_{M-1} - x_{M-2}, y_{M-1} - y_{M-2}) Normalize: v_norm = v / ||v|| (c) Position inference method: Case 1: If QC_{M-2} is online, infer based on the direction trend: x_M_init = x_{M-1} + d_avg × v_norm.x y_M_init = y_{M-1} + d_avg × v_norm.y Case 2: If only QC_{M-1} is online, use the average spacing and orientation: x_M_init = x_{M-1} + d_avg × cos(θ_{M-1}) y_M_init = y_{M-1} + d_avg × sin(θ_{M-1}) (d) Orientation inference: θ_M_init = θ_{M-1} (e) Get the initial inferred pose: Pose_M_init = (x_M_init, y_M_init, θ_M_init) (f) Inference accuracy evaluation: Expected position error: Δx ≈ σ_d For regularly arranged quay cranes, the error is usually <5m.

[0054] 6. Reliability evaluation of inference results Perform a reliability evaluation on the initial inference results: (a) Geometric consistency check: Check if the inferred position is within a reasonable range (within the port operation area boundary); (b) Spacing reasonableness check: Calculate the distance between the inferred position and the adjacent quay crane, and check if it is within a reasonable range: |d_actual - d_avg|<3σ_d If the range is exceeded, a warning will be issued; (c) Orientation rationality check: Check if the orientation angle is close to that of the adjacent quay bridge: |θ_n_init - θ_{n-1}|<15° (d) Reliability label: High reliability: intermediate position inference, and regular distance between adjacent quay bridges; Medium reliability: edge location inference, with more than 2 reference quay cranes; Low reliability: edge location inference, with only one reference quay crane.

[0055] 7. Application of the inference results The initial inferred pose Pose_n_init is as follows: (a) FMS temporary mission endpoint: guide the unmanned vehicle to its approximate location; (b) Initial values ​​for subsequent accurate estimation: serving as initial guesses for the ICP optimization algorithm; (c) Anomaly detection benchmark: Compare with the final accurate estimation result to assess its reasonableness.

[0056] Through the detailed initial inference method described above, those skilled in the art can use corresponding mathematical formulas and calculation steps to achieve the initial estimation of the pose of the disconnected quay bridge according to different quay bridge location types, providing reasonable initial values ​​for subsequent multi-sensor accurate estimation.

[0057] (a) Multi-sensor data preprocessing and time synchronization 1. Sensor Configuration and Calibration The multi-sensor system employed in this invention includes: one horizontal lidar at the front and rear (scanning frequency 10Hz), one vertical lidar (scanning frequency 10Hz), and one camera at the left, right, front, and rear (acquisition frequency 10Hz). The extrinsic parameter matrices of each sensor relative to the vehicle coordinate system {V} are obtained through offline calibration, with calibration accuracy requirements of: translation error <2cm, rotation error <0.5°.

[0058] 2. Time synchronization mechanism A hardware timestamp synchronization method is adopted, with all sensors synchronizing with the main control system via the PTP (Precision Time Protocol) protocol, achieving a time synchronization accuracy of <1ms. Data collected at different times is aligned to a unified timestamp t0 using linear interpolation. P(t0) = P(t1) + (t0-t1) / (t2-t1) × [P(t2) - P(t1)] where P represents sensor data, t1 < t0 < t2; P represents sensor data (which can be position coordinates, attitude angles, point cloud coordinates, etc.); t0 is the target unified timestamp, i.e., the target moment to which alignment is needed; t1 is a sampling time earlier than t0, and t2 is a sampling time later than t0, satisfying t1 < t0 < t2; P(t1) and P(t2) are respectively the sensor data collected at t1 and t2; P(t0) is the sensor data at t0 calculated by linear interpolation.

[0059] 3. Point cloud preprocessing The raw point cloud of the laser radar is preprocessed by the following steps: (a) Distance filtering: remove points with a distance > 50m or < 0.5m to avoid noise interference; (b) Statistical filtering: calculate the average distance of the K nearest neighbors (K = 50) for each point, and remove outliers with a distance > μ + 2σ, where μ is the mean and σ is the standard deviation; (c) Voxel filtering: downsample using a voxel grid of 0.05m x 0.05m x 0.05m to reduce computational load; (d) Ground removal: use the RANSAC plane fitting algorithm to remove ground points, the plane equation is ax + by + cz + d = 0, and points with a distance to the plane < 0.2m are considered as ground points.

[0060] 4. Image preprocessing The camera image is preprocessed as follows: (a) Distortion correction: perform distortion correction according to the distortion parameters obtained from calibration; (b) Gray scale normalization: normalize the image pixel values to the [0, 1] interval; (c) Gaussian filtering: smooth the image using a 5x5 Gaussian kernel to reduce noise.

[0061] (II) Specific method of multi-sensor feature extraction 1. Laser radar feature extraction (a) Shore bridge beam feature extraction The shore bridge beam appears as a horizontal strip structure in the point cloud, and the extraction steps are as follows: Step 1: Perform Euclidean clustering on the preprocessed point cloud, with a clustering distance threshold of 0.3m, to obtain multiple point cloud clusters; Step 2: Calculate the principal direction of each point cloud cluster using the PCA (Principal Component Analysis) method; Step 3: Calculate the eigenvalues λ1 ≥ λ2 ≥ λ3 and corresponding eigenvectors v1, v2, v3 of the covariance matrix C. Step 4: Determine if it is a beam feature: if λ1 / λ2>10 and λ2 / λ3>3, consider it as a long strip structure; Step 5: Further determine the direction: if the main direction v1 is <15° from the horizontal plane, consider it as a beam; Step 6: Extract the beam center line: the beam center point is P̄, and the direction is v1.

[0062] (b) Shore-to-ship vertical structure feature extraction The extraction of vertical support columns uses a similar method, with the following conditions: λ1 / λ2>10 and the main direction v1 is <15° from the vertical direction.

[0063] (c) Planar feature extraction Use RANSAC algorithm to extract planar features: Step 1: Randomly select 3 points to fit a plane equation ax+by+cz+d=0; Step 2: Calculate the distance of other points to the plane, and the points with a distance < threshold (0.05m) are inliers; Step 3: Repeat 1000 times, select the plane with the most inliers; Step 4: Refit the plane using all inliers to get the accurate plane parameters.

[0064] 2. Visual feature extraction (a) Edge feature extraction Use Canny edge detection algorithm: Step 1: Gaussian filter to smooth the image; Step 2: Calculate the image gradient: Gx = I ⊗ Sobel_x, Gy = I ⊗ Sobel_y Where: I is the input image; ⊗ represents convolution operation; Sobel_x and Sobel_y are the horizontal and vertical direction convolution kernels of Sobel operator respectively; Gx is the gradient component of the image in the x direction (horizontal direction); Gy is the gradient component of the image in the y direction (vertical direction).

[0065] Step 3: Non-maximum suppression: keep the local maximum in the gradient direction; Step 4: Double threshold detection: high threshold TH=100, low threshold TL=50, keep strong edges and weak edges connected to them.

[0066] (b) Straight line feature extraction Use Hough transform to extract straight lines on the edge image: Straight line parameter equation: ρ = x·cosθ + y·sinθ Where ρ is the distance from the origin to the straight line, and θ is the normal direction angle; The voting accumulator threshold is set to 50, and the straight line with the most votes is taken as the shore bridge structure edge.

[0067] (c) Corner feature extraction Harris corner detection or Shi-Tomasi corner detection is used for subsequent feature matching.

[0068] (Three) Specific algorithm of multi-sensor feature fusion 1. Coordinate system conversion Convert the features extracted by each sensor to the vehicle coordinate system {V}: For the feature points PL of the laser radar L, the conversion formula is: PV = RVL · PL + tVL Where RVL is the rotation matrix, tVL is the translation vector, and TVL is the transformation matrix: TVL = [RVL tVL] Where: PL is the feature point coordinate in the laser radar coordinate system (3x1 vector); PV is the feature point coordinate converted to the vehicle coordinate system {V} (3x1 vector); RVL is the rotation matrix from the laser radar coordinate system to the vehicle coordinate system (3x3 matrix); tVL is the translation vector from the laser radar coordinate system to the vehicle coordinate system (3x1 vector); "·" represents matrix multiplication.

[0069] For the feature points (pixel coordinates (u, v)) of the camera C, first project them to the camera coordinate system through the depth d; then convert them to the vehicle coordinate system: PV = TVC · PC PV: feature point coordinate converted to the vehicle coordinate system {V} (3x1 vector); PC: feature point coordinate converted to the camera coordinate system (3x1 vector); TVC: 4x4 homogeneous transformation matrix from the camera coordinate system to the vehicle coordinate system.

[0070] Where the correct coordinate transformation method for PC and TVC is as follows: For the feature points of the camera C, first convert the pixel coordinates (u, v) to 3D coordinates in the camera coordinate system through the depth information, and then convert them to the vehicle coordinate system through the homogeneous transformation matrix.

[0071] (1) Conversion of pixel coordinates to camera coordinate system: Let the pixel coordinates be (u, v), the depth be d, and the camera intrinsic matrix be K, then: PC = [X_c, Y_c, Z_c]ᵀ = d × K⁻¹ × [u, v, 1]ᵀ where X_c, Y_c, Z_c are the 3D coordinate components of the feature point in the camera coordinate system, X_c is the coordinate in the x-axis direction of the camera coordinate system (usually corresponding to the horizontal direction of the image), Y_c is the coordinate in the y-axis direction of the camera coordinate system (usually corresponding to the vertical direction of the image), Z_c is the coordinate in the z-axis direction of the camera coordinate system (usually corresponding to the depth direction, i.e., the direction of the camera optical axis), and PC is the 3D point coordinate in the camera coordinate system (3 x 1 vector).

[0072] (2) Transformation from the camera coordinate system to the vehicle coordinate system: A homogeneous coordinate transformation matrix TVC (4 x 4 matrix) is used, which is composed of a 3 x 3 rotation matrix RVC and a 3 x 1 translation vector tVC, where RVC represents the rotation transformation from the camera coordinate system to the vehicle coordinate system, and tVC represents the translation transformation.

[0073] (3) Homogeneous coordinate transformation: Expand PC to homogeneous coordinates PC_h = [X_c, Y_c, Z_c, 1]ᵀ (4 x 1 vector), then: PV_h = TVC × PC_h where PV_h = [X_v, Y_v, Z_v, 1]ᵀ is the homogeneous coordinate in the vehicle coordinate system; where X_v, Y_v, Z_v are the 3D coordinate components of the feature point in the vehicle coordinate system, X_v is the coordinate in the x-axis direction of the vehicle coordinate system (usually corresponding to the forward direction of the vehicle), Y_v is the coordinate in the y-axis direction of the vehicle coordinate system (usually corresponding to the left side direction of the vehicle), and Z_v is the coordinate in the z-axis direction of the vehicle coordinate system (usually corresponding to the vertical upward direction of the vehicle).

[0074] (4) Extract 3D coordinates: PV = [X_v, Y_v, Z_v]ᵀ = RVC × PC + tVC where PV is the feature point coordinate in the vehicle coordinate system (3 x 1 vector).

[0075] Therefore, the correct transformation formula should be: PV = RVC × PC + tVC Or using homogeneous coordinates: PV_h = TVC × PC_h, then extract the first three components to get PV.

[0076] 2. Feature association and matching (a) Geometric consistency check For the beam feature line L_lidar (center point P1, direction d1) extracted by lidar and the straight line feature L_camera (center point P2, direction d2) extracted by camera, the criteria for determining whether they are the same feature are: Distance constraint: ||P1 - P2|| < 0.5m Direction constraint: |d1 · d2| > cos(10°) ≈ 0.985 If the above conditions are met, it is considered that the feature pair is matched.

[0077] (b) Feature fusion weight calculation For the matched feature pair, a weighted fusion method is used to calculate the final feature position: P_fusion = wL·P_lidar + wC·P_camera The weight is dynamically adjusted according to the feature quality: wL = qL / (qL + qC) wC = qC / (qL + qC) P_fusion: fused feature position (3x1 vector); P_lidar: feature position extracted by lidar (3x1 vector); P_camera: feature position extracted by camera (3x1 vector); wL, wC: fusion weights of lidar and camera features (satisfy wL + wC = 1); qL, qC: quality scores of lidar and camera features; N_points: number of feature points; edge_strength: average edge strength; Where the quality score q is calculated as follows: Lidar feature quality: qL = N_points / 100, N_points is the number of feature points; Camera feature quality: qC = edge_strength / 100, edge_strength is the average edge strength.

[0078] (c) Multi-angle feature verification When multiple sensors observe the same pier feature (such as left camera and front camera at the same time), the consistency of multiple observations is calculated: Consistency score = 1 - std(P_obs1, P_obs2,..., P_obsn) / mean_distance Where: P obs1, P obs2,..., P obsn are the positions of the same feature observed by n different sensors (each is a 3x1 vector in vehicle coordinate system); std(·) is the standard deviation function, which calculates the standard deviation of n observed positions; mean_distance is the average distance of n observed points (the average distance from the origin to each observed point); the consistency score ranges from [0, 1], and the larger the value, the more consistent the multiple sensor observations are.

[0079] If the consistency score > 0.9, the feature is considered reliable.

[0080] (Four) Specific method of relative pose calculation 1. Relative pose calculation of disconnected shore bridge After multi-sensor fusion, the key feature point set {P feat1, P feat2,..., P featn} of the disconnected shore bridge is obtained (in the vehicle coordinate system {V}).

[0081] Where: P feat1, P feat2,..., P featn are the n key feature point coordinates after fusion (each is a 3x1 vector in the vehicle coordinate system).

[0082] The key feature point set {P feat1, P feat2,..., P featn} of the disconnected shore bridge is obtained through multi-sensor feature extraction and fusion, and the specific process is as follows: (1) Multi-sensor feature extraction: - Laser radar feature extraction: extract shore bridge beam features, vertical support column features, planar features, etc., to obtain the feature point set {P lidar1, P lidar2,..., P lidar_m} (in the laser radar coordinate system); - Visual feature extraction: extract edge features, straight line features, corner features, etc., to obtain the feature point set {P camera1, P camera2,..., P camera_k} (in the camera coordinate system).

[0083] (2) Coordinate system conversion: - Convert laser radar feature points to vehicle coordinate system: P lidar_V = RVL × P lidar + tVL - Convert camera feature points to vehicle coordinate system: P camera_V = RVC × P camera + tVC where P lidar V represents the feature point coordinates converted from the lidar coordinate system to the vehicle coordinate system (3x1 vector), P camera V represents the feature point coordinates converted from the camera coordinate system to the vehicle coordinate system (3x1 vector), both vectors are represented in the vehicle coordinate system {V} for subsequent feature fusion and relative pose calculation; - Obtain the feature point set in the vehicle coordinate system: {P lidar V1,..., P lidar Vm, P camera V1,..., P camera Vk} (3) Feature association and matching: - Match the same features from different sensors through geometric consistency check; - For matched feature pairs, use weighted fusion method to calculate the final feature position: P fusion = wL × P lidar V + wC × P camera V (4) Key feature point selection: - Select key feature points according to feature importance: a. Beam feature points: highest weight, all retained; b. Support column feature points: select 3-5 most prominent ones; c. Corner feature points: select the highest matching degree corner points; d. Edge feature points: select edge points related to the beam.

[0084] - Finally get n key feature points: {P feat1, P feat2,..., P featn}, where n is usually 5-15.

[0085] (5) Application of feature point set: These key feature point sets are used for subsequent relative pose calculation, and the center position and orientation of the quay crane are calculated through the geometric relationship of the feature points.

[0086] (a) Quay crane center position calculation Calculate the quay crane center position through the extracted beam features: Beam center point P beam = (1 / M)ΣPi, i∈Beam feature points Projection of quay crane center on ground: P qc_relative = [P beam.x, P beam.y, 0]T where P beam is the coordinate of the beam center point, P beam.x and P beam.y represent the coordinate components of the point in x-axis and y-axis directions, respectively.

[0087] The specific explanation is as follows: (1) Calculation of the center point P_beam of the beam: P_beam = (1 / M) × ΣPi, i∈beam characteristic points Where M is the number of feature points of the beam, and Pi is the coordinate (3×1 vector) of the i-th feature point of the beam.

[0088] (2) Coordinate representation of P_beam: P_beam = [P_beam.x, P_beam.y, P_beam.z]ᵀ in: - P_beam.x: The coordinate components (scalar values) of the center point of the beam in the x-axis direction; - P_beam.y: The coordinate components (scalar values) of the center point of the beam in the y-axis direction; - P_beam.z: The coordinate components (scalar values) of the center point of the beam in the z-axis direction.

[0089] (3) Projection of the center of the quay crane onto the ground: P_qc_relative = [P_beam.x, P_beam.y, 0]ᵀ This means projecting the center point of the beam onto the ground (the z=0 plane), retaining only the x and y coordinates, and setting the z coordinate to 0.

[0090] This is because the center position of a quay bridge is usually defined as its projection point on the ground.

[0091] (b) Calculation of quay bridge orientation Based on the direction vector d_beam of the crossbeam (already normalized), the quay crane's orientation angle is: θ_relative = atan2(d_beam.y, d_beam.x) (c) Relative pose representation Relative pose refers to the pose of the disconnected quay crane relative to the automated vehicle, including relative position and relative orientation; (1) Relative position: The projection of the quay crane center onto the ground is P_qc_relative=[P_beam.x,P_beam.y,0]ᵀ (in the vehicle coordinate system). Obtain the relative position vector: d_relative = [dx, dy, 0]ᵀ = P_qc_relative Where dx = P_beam.x and dy = P_beam.y represent the position of the quay crane center relative to the vehicle; (2) Relative orientation: Calculate the relative orientation angle according to the direction vector d_beam (normalized) of the beam: θ_relative = atan2(d_beam.y, d_beam.x) where atan2 is the four-quadrant arctangent function, returning an angle range of [-π, π]; (3) Relative pose representation: Pose_relative = (dx, dy, θ_relative) Or use the homogeneous transformation matrix T_relative to represent, which contains the rotation part (composed of cos(θ_relative) and sin(θ_relative)) and the translation part (dx, dy).

[0092] (Five) Specific method of global pose calculation (projection calculation) 1. Coordinate system definition Global coordinate system {G}: port fixed coordinate system; Vehicle coordinate system {V}: local coordinate system of automated vehicle; Quay crane coordinate system {QC}: local coordinate system of disconnected quay crane.

[0093] 2. Vehicle global pose acquisition The relative pose represents the pose of the disconnected quay crane relative to the automated vehicle, including the relative position and the relative orientation.

[0094] (1) Relative position: Through the projection P_qc_relative = [P_beam.x, P_beam.y, 0]ᵀ of the quay crane center on the ground (in the vehicle coordinate system), get the relative position vector: d_relative = [dx, dy, 0]ᵀ = P_qc_relative where dx = P_beam.x, dy = P_beam.y, indicating the position of the quay crane center relative to the vehicle; (2) Relative orientation: Calculate the relative orientation angle according to the direction vector d_beam (normalized) of the beam: θ_relative = atan2(d_beam.y, d_beam.x) where atan2 is the four-quadrant arctangent function, returning an angle range of [-π, π]; (3) Relative pose representation: Pose_relative = (dx, dy, θ_relative) or using a homogeneous transformation matrix T_relative, which contains a rotation part (composed of cos(θ_relative) and sin(θ_relative)) and a translation part (dx, dy).

[0095] 3. Global pose projection calculation Quay crane pose in global coordinate system = Vehicle pose in global coordinate system × Quay crane pose relative to vehicle.

[0096] Convert the relative pose to global pose through coordinate transformation.

[0097] (1) Coordinate system definition: - Global coordinate system {G}: Port-fixed coordinate system, origin usually set at the port center or some fixed reference point; - Vehicle coordinate system {V}: Origin at the vehicle center, x-axis pointing in the vehicle's forward direction, y-axis pointing to the vehicle's left side; (2) Coordinate transformation formula: Quay crane pose in global coordinate system is calculated by the following formula: Position transformation: x_qc = x_v + dx × cos(θ_v) - dy × sin(θ_v) y_qc = y_v + dx × sin(θ_v) + dy × cos(θ_v) Orientation transformation: θ_qc = θ_v + θ_relative Where: - (x_qc, y_qc, θ_qc): Disconnected quay crane pose in global coordinate system; - (x_v, y_v, θ_v): Vehicle pose in global coordinate system; - (dx, dy, θ_relative): Quay crane pose relative to vehicle; (3) Matrix representation: Use homogeneous transformation matrix for coordinate transformation: T_global = T_vehicle × T_relative where T_vehicle is the homogeneous transformation matrix of the vehicle global pose, containing the rotation part (composed of cos(0_v) and sin(0_v)) and the translation part (x_v, y_v); T_relative is the homogeneous transformation matrix of the relative pose, containing the rotation part (composed of cos(0_relative) and sin(0_relative)) and the translation part (dx, dy). The global pose homogeneous transformation matrix T_global is obtained by matrix multiplication, from which the pose (x_qc, y_qc, 0_qc) of the disconnected shore bridge in the global coordinate system can be extracted; (4) Calculation results: The preliminary pose estimation of the disconnected shore bridge in the global coordinate system is obtained: Pose_initial = (x_qc, y_qc, 0_qc) This pose serves as the initial value for subsequent ICP optimization.

[0098] The relative pose is converted to the global pose by matrix multiplication.

[0099] (Six) Specific method of accurate pose optimization This step uses the ICP (Iterative Closest Point) point cloud registration algorithm to accurately match the real-time detected shore bridge point cloud with the pre-built map.

[0100] 1. Pre-built map loading According to the disconnected shore bridge ID, load the pre-built point cloud map M = {m1, m2,..., mN} of the shore bridge from the database, and the point cloud format is (x, y, z) coordinates. The pre-built map is represented in the global coordinate system.

[0101] 2. Real-time point cloud acquisition Extract the currently scanned disconnected shore bridge point cloud S = {s1, s2,..., sK}, which is in the vehicle coordinate system and needs to be converted to the global coordinate system first: si_global = R_V_G × si_local + t_V_G Wherein: S is a real-time scanned disconnected shore-to-bridge point cloud set, containing K points; s1, s2,..., sK are each point in the point cloud (each point is a 3x1 vector in the vehicle coordinate system); si_local is the coordinates of the i-th point in the vehicle coordinate system (3x1 vector, i=1, 2,..., K); si_global is the coordinates of the i-th point converted to the global coordinate system (3x1 vector); R_V_G is a 3x3 rotation matrix from the vehicle coordinate system {V} to the global coordinate system {G}; t_V_G is a translation vector (3x1 vector) from the vehicle coordinate system {V} to the global coordinate system {G}; "x" represents matrix multiplication; "+" represents vector addition. The formula realizes the coordinate transformation of the point cloud from the vehicle coordinate system to the global coordinate system.

[0102] 3. ICP algorithm iteration optimization 4. ICP algorithm improvement In order to improve the robustness, the following improvement measures are adopted in the application: (a) Point-to-plane ICP: not only considering the point-to-point distance, but also considering the point-to-local plane distance; (b) Weighted ICP: according to the importance of features, different weights are given, the weight of beam features is set to 1.5, and the weight of other features is 1.0; (c) Multi-resolution ICP: first use down-sampling point cloud to quickly converge, and then use fine point cloud to accurately optimize.

[0103] (Seven) specific calculation method of confidence evaluation In order to ensure the reliability of the pose estimation, a multi-dimensional confidence evaluation system is designed in the application.

[0104] Point cloud matching degree The matching degree of real-time point cloud and map is measured. The calculation method is: the number of points in the real-time point cloud with a distance less than a threshold value from the corresponding points in the pre-built map is counted, and then divided by the total number of real-time point cloud to obtain the matching degree. The distance threshold is set to 0.1 meters, that is, when the point in the real-time point cloud is transformed optimally and the distance between the nearest neighbor point in the map is less than 0.1 meters, it is considered that the point is matched successfully. The matching degree is in the range of 0 to 1, and in typical cases, the matching degree greater than 0.8 indicates good matching.

[0105] 2. Pose convergence degree C_converge Convergence quality of ICP optimization is measured. The calculation method is: according to the pose change of the last iteration, the comprehensive change is calculated. The comprehensive change comprehensively considers the translation change and the rotation change, wherein the angle change is converted into an equivalent displacement through a scale factor (set as 10 meters), and then the comprehensive value of the translation change and the equivalent rotation change is calculated. The convergence degree is calculated through an exponential function, and the smaller the comprehensive change is, the higher the convergence degree is. The convergence degree takes a value in the range of 0 to 1, and in a typical case, the convergence degree greater than 0.9 indicates good convergence.

[0106] 3. Sensor consistency C_sensor The consistency of multi-sensor observation is measured. The calculation method is: the distance deviation between the position of the shore crane estimated by the lidar and the position of the shore crane estimated by the camera is calculated, divided by the maximum allowed deviation (set as 2.0 meters), and then the consistency score is obtained by subtracting the ratio from 1. If the distance deviation of the position estimated by the two sensors exceeds the maximum allowed deviation, the consistency score is directly set to 0. The consistency score takes a value in the range of 0 to 1, and in a typical case, the consistency score greater than 0.85 indicates good consistency.

[0107] 4. Feature richness C_feature The calculation method is: the number of successfully extracted and fused features is divided by the minimum required number of features (set as 5), and the smaller value of the ratio and 1.0 is taken as the feature richness. The feature richness takes a value in the range of 0 to 1, and when the number of features reaches or exceeds the minimum required number, the feature richness is 1.0, indicating that the features are sufficient.

[0108] 5. Total confidence C_total The total confidence is calculated by a weighted comprehensive method. The point cloud matching degree, pose convergence degree, sensor consistency and feature richness are weighted and summed according to the weights. The weights are determined according to experience and experiments: the weight of the point cloud matching degree is 0.4 (most important), the weight of the pose convergence degree is 0.3 (second), the weight of the sensor consistency is 0.2, and the weight of the feature richness is 0.1. The sum of the four weights is 1.0. The total confidence takes a value in the range of 0 to 1, and the larger the value is, the more reliable the pose estimation result is.

[0109] 6. Confidence judgment rule According to the value of the total confidence C_total, the reliability of the estimation result is judged: (a) C_total>0.9: high confidence, directly accept the estimation result; (b) 0.7 (c) C_total ≤ 0.7: low confidence, reject the estimation result, trigger re-scanning or use a degradation strategy.

[0110] (Eight) Specific method of rationality verification Compare the optimized pose with the initial pose inferred by FMS to verify the rationality of the result.

[0111] Pose includes Position and Orientation, and when comparing and analyzing, the position deviation and angle deviation need to be calculated respectively: Pose = (x, y, θ) Where (x, y) is the position coordinate, and θ is the orientation angle.

[0112] The comparison method is to compare the optimized pose Pose_opt = (x_opt, y_opt, θ_opt) and the initial pose Pose_fms = (x_fms, y_fms, θ_fms) inferred by FMS in the position and orientation dimensions respectively.

[0113] 1. Position deviation calculation Calculate the Euclidean distance; calculate the Euclidean distance between the optimized shore bridge position coordinates and the initial position coordinates inferred by FMS, and get the position deviation. The position deviation represents the straight-line distance between two position points.

[0114] 2. Angle deviation calculation (orientation dimension comparison) Calculate the difference between the optimized shore bridge orientation angle and the initial orientation angle inferred by FMS to get the angle deviation. If the angle difference is greater than 180 degrees, take 360 degrees minus the difference as the angle deviation (take the smaller angle). The angle deviation is expressed in degrees.

[0115] Calculate the orientation angle difference: Δθ = |θ_opt - θ_fms| Where: θ_opt is the optimized shore bridge orientation angle θ_fms is the orientation angle in the initial pose inferred by FMS Angle normalization: If Δθ > π, then Δθ = 2π - Δθ (take the smaller angle) Convert to angle system: Δθ_deg = Δθ × 180 / π 3. Adjacent shore bridge distance verification Check if the distance between the disconnected shore bridge and its adjacent shore bridges is reasonable. If both the previous and next shore bridges of the disconnected shore bridge are online, calculate the expected distance (half of the distance between two adjacent shore bridges) and the actual distance (distance from the optimized shore bridge position to the position of the previous shore bridge), and calculate the distance deviation rate (absolute value of the difference between the actual distance and the expected distance divided by the expected distance).

[0116] Check if the distance between the disconnected shore bridge and its adjacent shore bridges is reasonable: If both the n-1 and n+1 shore bridges are online: Expected distance: d_expected = |pos_n+1 - pos_n-1| / 2 Actual distance to n-1: d_actual = ||Pose_opt - Pose_n-1|| Distance deviation rate: η = |d_actual - d_expected| / d_expected Where: (1) pos_n+1 and pos_n-1: pos_n+1 represents the position coordinates of the n+1 shore bridge (2D position, excluding angle), i.e.: pos_n+1 = (x_{n+1}, y_{n+1}) pos_n-1 represents the position coordinates of the n-1 shore bridge, i.e.: pos_n-1 = (x_{n-1}, y_{n-1}) These two position coordinates are extracted from the position part of the shore bridge pose.

[0117] (2) Pose_n-1: Pose_n-1 represents the complete pose of the n-1 shore bridge (including position and orientation), i.e.: Pose_n-1 = (x_{n-1}, y_{n-1}, θ_{n-1}) Where: - x_{n-1}: x-coordinate of the n-1 shore bridge in the global coordinate system; - y_{n-1}: y-coordinate of the n-1 shore bridge in the global coordinate system; - θ_{n-1}: orientation angle of the n-1 shore bridge in the global coordinate system.

[0118] (3) Application in distance verification: In the distance verification formula of paragraph 0091: - |pos_n+1 - pos_n-1|: represents the Euclidean distance between the n+1th and n-1th quayside cranes, i.e.: |pos_n+1 - pos_n-1| = sqrt((x_{n+1} - x_{n-1})² + (y_{n+1} - y_{n-1})²) - ||Pose_opt - Pose_n-1||: represents the position distance between the optimized lost connection quayside crane pose and the n-1th quayside crane pose, i.e.: ||Pose_opt - Pose_n-1|| = sqrt((x_opt - x_{n-1})² + (y_opt - y_{n-1})²) Note: Here only the position part of the distance is calculated, without considering the angle difference.

[0119] (4) Symbol explanation: - Pos_{n+1} or Pose_{n+1} represents the pose of the n+1th quayside crane; - Pos_{n-1} or Pose_{n-1} represents the pose of the n-1th quayside crane; - pos_{n+1} and pos_{n-1} only represent the position coordinates (without angle).

[0120] 4. Reasonable threshold setting According to the actual situation of the port and the system accuracy requirement, set the threshold: Position deviation threshold: Δd_max = 5.0m (quayside crane movement range is usually not more than this value) Angle deviation threshold: Δθ_max = 10° (quayside crane orientation change range) Distance deviation rate threshold: η_max = 0.3 (30%) According to the actual situation of the port and the system accuracy requirement, set the reasonable threshold: the position deviation threshold is 5.0 meters (the quayside crane movement range is usually not more than this value), the angle deviation threshold is 10 degrees (the quayside crane orientation change range), and the distance deviation rate threshold is 30%.

[0121] 5. Reasonable judgment logic If all the following conditions are met, the estimated result is considered reasonable: (a) Position deviation is less than 5.0 meters; (b) Angle deviation is less than 10 degrees; (c) If there is adjacent quayside crane information, distance deviation rate is less than 30%; (d) Comprehensive confidence is greater than 0.7.

[0122] 6. Exception Handling Strategy If the rationality verification fails, the following measures will be taken: (a) Record abnormal information, including deviation values, confidence levels, sensor status, etc.; (b) Require the vehicle to adjust its position and rescan from different angles (maximum of 3 retries). (c) If multiple retries still fail, report the abnormal status to FMS, and: - Use the initial position inferred by FMS as the degradation scheme; - Reduce vehicle speed to increase safety margin; - Notify the operator to intervene manually.

[0123] (ix) Results reporting and dynamic update mechanism 1. Data encapsulation 2. Communication Protocol It communicates with FMS using TCP / IP or ROS (Robot Operating System) messaging mechanism, with a communication frequency of 10Hz (detection phase) or event triggering (pose update).

[0124] 3. FMS Dynamic Update Process (a) FMS receives pose estimation results; (b) Verify message integrity and timeliness (timestamp delay < 1s); (c) Update the internal quay crane pose database; (d) Replan the task endpoint: Update the temporary endpoint to the optimized pose; (e) Issue the updated task to the vehicle in operation; (f) If the vehicle is close to the original destination, perform trajectory smoothing to avoid sharp turns.

[0125] 4. Real-time monitoring and feedback The system continuously monitors the pose estimation quality: (a) If the confidence level continues to decline, the warning sensor may be malfunctioning; (b) If multiple verifications fail, an early warning will be issued indicating that the quay bridge may be moving or that the environment may be changing; (c) Periodically (every 5 minutes) re-estimate the pose to ensure data real-time performance.

[0126] Through the detailed technical implementation methods described above, those skilled in the art can fully implement this invention, achieving high-precision and high-reliability pose estimation for disconnected quay cranes. In actual port environment testing, the system achieved a position accuracy of ±0.05m, an angle accuracy of ±0.2°, and a confidence level generally >0.9, meeting the stringent requirements of port automation operations.

Claims

1. A shore-based bridge pose estimation fault-tolerant method based on multi-sensor fusion, characterized in that, The method comprises the following steps: 1) Pre-registering the shore crane position information and establishing a shore crane position database; 2) Real-time receiving of shore crane position data and comparison and analysis with the registered information; 3) Detection of shore crane position disconnection, and immediate error reporting and recording when a missing shore crane position is found; 4) Analysis of the possible position range of the missing shore crane, and prediction based on historical data and adjacent shore crane information; 5) Calculation of the temporary task end point of the n-th shore crane by using the known position information of the adjacent n-1-th and n+1-th shore cranes; 6) When the unmanned vehicle arrives at the n-1-th shore crane, the n-th shore crane is scanned at the marginal position by multiple sensors, which are laser radars and cameras; 7) Multi-sensor feature fusion: fusion of the shore beam features extracted by multiple laser radars and cameras to obtain the relative position of the disconnected shore crane relative to the automated vehicle; 8) Global position calculation: combining the current frame position of the automated vehicle to obtain the approximate position of the disconnected shore crane in the global coordinate system; 9) Precise position optimization: optimization of the accurate position of the disconnected shore crane in the global coordinate system by using the shore crane point cloud detected by the automation in real time and the pre-built shore crane point cloud map of the disconnected shore crane; 10) Reporting the optimized position result to the FMS system, and dynamically updating the n-th shore crane navigation position point by the FMS system.

2. The multi-sensor fusion based container crane pose estimation fault-tolerant method according to claim 1, characterized in that, The specific implementation of steps 5) to 10) is as follows: (1) When the FMS receives the n-th shore crane disconnection notification; (2) Determining the position type of the n-th shore crane based on the shore crane area sequence, and judging whether it is a middle position or an edge position; (3) Using different inference strategies according to the position type: a. If in the middle position: using the middle value of the adjacent shore cranes before and after as the initial position; b. If in the edge position: inferring the approximate position based on the adjacent shore crane positions and the overall trend; (4) The FMS first sends the inferred initial position as a temporary end point; (5) When the automated vehicle arrives at the n-1-th shore crane, the n-th shore crane is scanned at the marginal position by laser radars and visual sensors; (6) Based on the global position of the automated vehicle under the n-1-th shore crane, the absolute position of the n-th shore crane is calculated by projection; (7) Reporting the estimated result to the FMS, and dynamically updating the task end point to the estimated n-th shore crane position by the FMS.

3. The Shore Cranes Pose Estimation Fault-Tolerant Method Based on Multi-Sensor Fusion according to claim 2, characterized in that, If the FMS dynamically updates the new task of the n-th shore crane position in step (7), multi-sensor scanning and position estimation are performed, which are specifically performed according to the following steps: (1) Multi-sensor fusion system: composed of multiple laser radars and multiple camera systems, the multiple laser radars are configured to work cooperatively with front and rear horizontal lasers and vertical laser radars; the multiple camera system is a full-range coverage system composed of left, right, front and rear cameras; the sensor data fusion is intelligent fusion of laser point cloud and visual features, and the time alignment and synchronization of multi-sensor data; (2) First, the multi-sensor features are extracted, which are respectively: Laser radar feature extraction: shore crane vertical structure, beam feature, support column feature; Visual feature extraction: shore crane identification, color feature, texture feature, edge feature; Secondly, the features are fused: intelligent association and matching of multi-sensor features; Finally, the features are verified: consistency verification of multi-angle features; (3) Pose optimization: First, real-time point cloud and pre-built map matching: accurate matching of current detected point cloud and historical map; then global pose optimization: global pose calculation based on multi-sensor fusion; again confidence assessment: multi-dimensional confidence calculation and verification; finally dynamic update: dynamic update of real-time pose information; (4) Reasonable verification of the above updated real-time pose information, the specific steps are as follows: a. Initial pose comparative analysis: comparing the pose calculated by multi-sensor fusion with the initial position inferred by FMS; b. Deviation calculation: calculating the position deviation and angle deviation between the calculated pose and the inferred initial position; c. Reasonability judgment: based on the preset deviation threshold, judge whether the calculation result is reasonable; the verified disconnected shore-to-ship bridge position reports the estimated result to FMS: dynamically updates the task end position and ensures the continuity of the unmanned vehicle task.

Images