A shore-based bridge pose estimation fault-tolerant method based on multi-sensor fusion

Abstract

This invention discloses a fault-tolerant method for quay crane pose estimation based on multi-sensor fusion, belonging to the technical field of port automation equipment. When a quay crane loses connection due to a network failure, the system automatically detects the pose loss by comparing and analyzing pre-registered quay crane IDs and placement order information with real-time received data. Upon detecting a missing quay crane position, the system immediately reports an error and records the disconnection information. Different intelligent inference strategies are employed based on the quay crane's position order within the area: for quay cranes in the middle position, the median value of the adjacent quay cranes is used as the initial pose; for quay cranes at the edge position, the approximate pose is inferred based on the positions of adjacent quay cranes and the overall trend. This invention employs a point cloud matching-based pose estimation algorithm, achieving high-precision relative pose estimation through multi-sensor fusion of LiDAR and visual cameras, significantly improving the fault tolerance and operational continuity of the port automation system.

Claims

1. A fault-tolerant method for quay crane pose estimation based on multi-sensor fusion, characterized in that, Includes the following steps: 1) Pre-register the position and orientation information of the quay cranes and establish a quay crane position and orientation database; 2) Receive the position and orientation data of the quay crane in real time and compare and analyze it with the registration information; 3) Detect the disconnection of quay crane pose; when a missing quay crane pose is found, report the error immediately and record it. 4) Analyze the pose range of the missing quay bridge based on historical data and pose information of adjacent quay bridges; 5) Based on the location type of the missing quay crane in the quay crane region, calculate the initial temporary task endpoint of the missing quay crane using the known pose information of adjacent quay cranes, where: 5-1) When the missing quay bridge is a quay bridge in the middle position, the known poses of the adjacent n-1 and n+1 quay bridges are used to interpolate the poses of the two to obtain the initial pose of the missing n quay bridge. 5-2) When the missing quay bridge is the quay bridge at the first edge position, the initial pose of the nth quay bridge is calculated using the known pose of the adjacent n+1th quay bridge and the average spacing between quay bridges obtained based on historical data statistics. 5-3) When the missing quay bridge is at the end edge position, the initial pose of the nth quay bridge is calculated using the known pose of the adjacent n-1th quay bridge and the average spacing between quay bridges obtained based on historical data statistics. 6) The initial pose is sent to the unmanned vehicle as a temporary task endpoint. When the unmanned vehicle reaches the edge position of the reference quay bridge adjacent to the nth quay bridge, the nth quay bridge is scanned by multiple sensors, including lidar and camera. 7) Perform feature extraction and fusion on the data collected by the multi-sensor, and fuse the features of the quay crane beams extracted by multiple lidar and camera sensors to obtain the relative pose of the disconnected quay crane relative to the automated vehicle. 8) Based on the pose of the automated vehicle in the current frame and the relative pose, perform global pose calculation to obtain the preliminary pose of the disconnected quay bridge in the global coordinate system. 9) Match and optimize the preliminary pose with the point cloud of the quay bridge detected in real time by the unmanned vehicle and the pre-constructed point cloud map of the disconnected quay bridge to obtain the optimized pose of the disconnected quay bridge in the global coordinate system. 10) The optimized pose results are reported to the FMS system, which then dynamically updates the navigation pose of quay bridge n.

2. The fault-tolerant method for quay crane pose estimation based on multi-sensor fusion according to claim 1, characterized in that, The specific implementation of steps 5) to 10) is as follows: (1) When FMS receives the disconnection notification from quay bridge number n; (2) Determine the location type of quay crane n based on the order of the quay cranes in the port area, and judge whether it is an intermediate location or an edge location; (3) Different initial pose inference strategies are adopted according to the position type: a. When the nth quay crane is in the middle position, obtain the poses of the adjacent quay cranes in front and behind, and use the median of the poses of the adjacent quay cranes in front and behind as the initial pose of the nth quay crane. b. When the nth quay crane is located at the edge, infer the initial pose of the nth quay crane based on the poses of the adjacent quay cranes, the statistically obtained average spacing between the quay cranes, and the trend of the quay crane arrangement direction; (4) The FMS generates a temporary task endpoint based on the initial pose and sends it to the automated vehicle; (5) When the automated vehicle reaches the edge position of the reference quay adjacent to the nth quay, it scans the nth quay at that position using lidar and visual sensors to extract the quay features. (6) Based on the global pose of the automated vehicle under the reference quay bridge and the relative pose of the disconnected quay bridge to the vehicle, coordinate transformation is performed to calculate the absolute pose of quay bridge n. (7) The absolute pose result is reported to FMS, and FMS dynamically updates the mission endpoint to the pose of quay bridge n.

3. The fault-tolerant method for quay crane pose estimation based on multi-sensor fusion according to claim 2, characterized in that, If step (7) is to execute a new task for the nth quay bridge location dynamically updated by the FMS, multi-sensor scanning and pose estimation are performed, specifically according to the following steps: (1) Multi-sensor fusion system: It consists of multiple lidar and multiple camera systems. The multiple lidar is configured to work in coordination with front and rear horizontal lidar and vertical lidar. The multiple camera system provides all-round coverage with left, right, front and rear cameras. Sensor data fusion is the intelligent fusion of lidar point cloud and visual features, and the time alignment and synchronization of multiple sensor data. (2) First, the features of multiple sensors are extracted, namely: LiDAR feature extraction: vertical structure of the quay crane, beam features, and support column features; visual feature extraction: quay crane markings, color features, texture features, and edge features; second, the features are fused: intelligent association and matching of multi-sensor features; finally, the features are verified: consistency verification of multi-angle features. (3) Perform pose optimization: First, perform real-time point cloud matching with pre-built map: precise matching of the current detected point cloud with the historical map; then perform global pose optimization: global pose calculation based on multi-sensor fusion; next, perform confidence evaluation: multi-dimensional confidence calculation and verification; finally, perform dynamic update: dynamic update of real-time pose information. (4) Verify the rationality of the updated real-time pose information as follows: a. Initial pose comparison and analysis: The pose calculated by multi-sensor fusion is compared and analyzed with the initial pose inferred by FMS; b. Deviation calculation: Calculate the positional and angular deviations between the estimated pose and the inferred initial pose; c. Reasonableness judgment: Based on the preset deviation threshold, judge whether the estimation result is reasonable; The verified position of the disconnected quay bridge is reported to the FMS in real time: dynamically update the mission endpoint position and ensure the continuity of the unmanned vehicle mission.

Images