Texture point cloud-based berthing and leaving self-positioning method and system in intelligent ship

Abstract

The invention relates to an intelligent ship berthing and leaving self-positioning method and system based on texture point clouds, and the method comprises the steps: firstly employing a fisheye camera to shoot a checkerboard calibration plate at multiple angles, employing a Zhang's calibration method to calculate an internal reference matrix and a distortion coefficient of the fisheye camera, and synchronously collecting the data of a laser radar and the camera in real time; the method comprises the following steps: carrying out orthodontic processing on a camera image, calculating an external parameter matrix between a laser radar and a camera through a feature extraction algorithm, a feature point matching algorithm and a nonlinear optimization algorithm, and carrying out data fusion through an internal parameter matrix, the external parameter matrix and a bilinear interpolation method to generate texture point cloud data with color information; the method comprises the following steps: performing texture point cloud registration through an improved NDT-color algorithm in cooperation with open-loop mileage estimation to preliminarily determine the position change of a ship, optimizing a graph model by using a closed-loop detection and graph optimization technology, generating an optimized global displacement vector, and further generating a wharf high-precision map containing color texture information. And real-time self-positioning of the ship in a wharf high-precision map is realized.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent ship navigation, and in particular to a texture point cloud-based berthing and unberthing self-positioning method and system in an intelligent ship. Background Art

[0002] Accurate positioning and mapping are crucial for intelligent ship docking and undocking. Traditional SLAM (Simultaneous Localization and Mapping) algorithms have numerous limitations when applied to the marine sector. On the one hand, algorithms that rely solely on LiDAR (LiDAR) fail to provide intuitive environmental information, preventing crew members from quickly identifying key targets. On the other hand, visual SLAM algorithms that rely solely on cameras lack precise distance measurement and are prone to positioning drift and insufficient map accuracy in complex and changing dock environments, seriously impacting the safety and efficiency of docking and undocking.

[0003] Furthermore, current ship berthing and unberthing systems rely on the following methods: 1) Multi-sensor fusion-based berthing and unberthing systems focus on condition monitoring, lack a complete and efficient SLAM process, and lack the use of structured light and other technologies to achieve high-precision 3D model construction and accurate odometry calculations. This inability to provide continuous and stable positioning and navigation support for ships makes it difficult to meet the needs of complex terminal operations. 2) Reliance on drone-assisted point cloud colorization is complex and unstable, and lacks effective means for closed-loop detection and back-end optimization. The resulting maps are not updated in a timely manner, have poor accuracy, and cannot adapt to dynamic changes at the terminal, such as temporary docking of ships and cargo loading and unloading.

[0004] In summary, existing means still cannot fully meet the ship's berthing and unberthing requirements for high-precision, intuitive and real-time maps. There is an urgent need for a more efficient and reliable ship self-positioning solution to provide accurate navigation information for ship berthing and unberthing operations. Summary of the Invention

[0005] In order to solve the problems that the point cloud data of ships in the process of berthing and unberthing at the dock lacks intuitive color texture information, making it impossible for crew members to intuitively understand the point cloud data, and the lack of accurate distance measurement leads to positioning drift and insufficient map accuracy in the dock environment. The present invention provides a method for berthing and unberthing self-positioning based on texture point cloud in intelligent ships. The method can use a camera to give color texture to the lidar point cloud, making the point cloud data intuitive and easy to understand. Through open-loop mileage estimation, the displacement change between adjacent frames of the ship is calculated based on the point cloud alignment result, and the position change of the ship in the berthing and unberthing process is preliminarily determined, so that the crew can quickly identify the target, greatly shorten the decision-making time, and improve the efficiency and safety of berthing and unberthing operations. At the same time, with the help of closed-loop detection and graph model optimization, accurate distance measurement can be provided to construct a high-precision map of the dock, realizing real-time self-positioning of the ship, thereby providing accurate navigation information for the ship's berthing and unberthing operations in all directions, meeting the needs of the entire process of complex dock operations. The present invention also relates to a berthing and unberthing self-positioning system based on texture point cloud in intelligent ships.

[0006] The technical solutions of the present invention are as follows:

[0007] A method for berthing and unberthing self-positioning in an intelligent ship based on texture point cloud, characterized by comprising the following steps:

[0008] Checkerboard image acquisition steps: Use a fisheye camera to shoot a checkerboard calibration plate pre-set at a fixed position on the dock from different angles to obtain multiple checkerboard images;

[0009] Intrinsic calibration: A computer vision algorithm is used to detect the 2D pixel coordinates of all inner corner points of each checkerboard grid in each checkerboard grid image. The 3D world coordinates corresponding to the 2D pixel coordinates of each inner corner point are then established based on the physical dimensions of the checkerboard grid in each checkerboard grid image. Based on the 2D pixel coordinates and 3D world coordinates of each inner corner point, the Zhang calibration method is used to calculate the intrinsic parameter matrix and distortion coefficient of the fisheye camera, completing the intrinsic calibration of the fisheye camera.

[0010] Environmental data synchronous collection and orthogonal processing steps: The environmental data around the dock is collected synchronously and in real time using a LiDAR and a calibrated fisheye camera. Multiple point cloud data collected by the LiDAR and multiple environmental images collected by the calibrated fisheye camera are obtained. The environmental images are then orthogonally processed using distortion coefficients to obtain the orthogonalized environmental images.

[0011] The steps for calculating the extrinsic parameter matrix are as follows: based on the feature extraction algorithm, the coordinates of all 3D edge feature points in the point cloud data in the laser radar coordinate system and the coordinates of all 2D feature points in the orthogonalized environment image in the camera coordinate system are extracted respectively, and the point cloud feature set and the environment image feature set are obtained respectively. The 3D edge feature points in the point cloud feature set are matched with the 2D feature points in the environment image feature set using the feature point matching method to obtain matched feature point pairs; the matched feature point pairs are then optimized using the RANSAC optimization algorithm to obtain optimized feature point pairs, and the extrinsic parameter matrix of the laser radar relative to the camera is calculated based on the optimized feature point pairs and a nonlinear optimization algorithm;

[0012] Point cloud texture mapping fusion steps: The coordinates of all 3D edge feature points in the point cloud feature set in the lidar coordinate system are converted to the camera coordinate system through the external parameter matrix to obtain the coordinates of all 3D edge feature points in the camera coordinate system, and the coordinates of all 3D edge feature points in the camera coordinate system are projected onto the orthogonalized environment image through the internal parameter matrix to obtain the corresponding projected pixel coordinates; the RGB color value of each projected pixel coordinate is extracted from the orthogonalized environment image using the bilinear interpolation method, and the RGB color value is assigned to the corresponding 3D edge feature point according to the spatial mapping rule, thereby generating texture point cloud data with color information and texture information;

[0013] Data registration and open-loop mileage estimation steps: Based on the NDT algorithm, the geometric features and RGB color values in the texture point cloud data are introduced to construct an improved NDT-color algorithm, and the improved NDT-color algorithm is used to align and register each 3D edge feature point in the texture point cloud data of two adjacent frames to obtain the registered texture point cloud data of each frame; then, based on the coordinates of each 3D edge feature point in the registered texture point cloud data, the center coordinates of each frame of the registered texture point cloud data are calculated; according to the center coordinates of the current frame and the previous frame of the current frame, the displacement vector of the ship in the current frame relative to the previous frame is calculated, and then the displacement vector between each pair of adjacent frames from the starting frame to the current frame is obtained, and then all displacement vectors from the starting frame to the current frame are accumulated to achieve open-loop mileage estimation and form a preliminary position trajectory;

[0014] Closed loop detection and graph model optimization steps: Use the texture point cloud data after each frame registration as nodes and the displacement vectors between each pair of adjacent frames in the preliminary position trajectory as edges to construct a graph model; every certain number of frames, detect whether there is a closed loop in the 3D edge feature points in the registered texture point cloud data. If a closed loop is detected, add a closed loop constraint to the graph model, and compare the number of closed loop constraints in the graph model with a preset number threshold. If the closed loop constraint in the graph model is greater than or equal to the preset number threshold, use the G2O graph optimization algorithm to optimize the graph model and generate an optimized global displacement vector.

[0015] Steps for constructing a high-precision map of the terminal: Use the optimized global displacement vector to correct the position of each 3D edge feature point in the registered texture point cloud data of each frame to obtain the corrected texture point cloud data of each frame; then generate a high-precision map of the terminal containing color and texture information based on the corrected texture point cloud data of all frames;

[0016] Real-time self-positioning steps: During the berthing and unberthing process of the ship, the point cloud data currently collected by the lidar is compared with the high-precision map of the terminal in real time, and the feature matching algorithm and positioning algorithm are used to determine the position of the ship in the high-precision map of the terminal in real time, thereby realizing the real-time self-positioning of the ship in the high-precision map of the terminal.

[0017] Preferably, in the closed-loop detection and graph model optimization steps, g2o is first used to construct a graph model representing the geometric constraints and feature matching relationships between point cloud data, with the texture point cloud data after registration of each frame as nodes and the displacement vectors between each pair of adjacent frames in the preliminary position trajectory as edges; a closed-loop detection is started every 20 frames of laser point cloud are collected, and the 3D edge feature points in the registered texture point cloud data are analyzed to determine whether a closed loop exists; when the number of closed-loop constraints in the graph model is greater than or equal to a preset number threshold, the g2o graph optimization algorithm and the NDT-color algorithm are comprehensively used to optimize the graph model, and by adjusting the constraint relationship between nodes in the graph model, the accumulated error is detected and corrected, and the optimized mileage estimate is regenerated, and an optimized global displacement vector is generated based on the optimized mileage estimate.

[0018] Preferably, in the step of constructing the high-precision map of the terminal, after position correction is performed on each 3D edge feature point, a filtering algorithm is used to remove noise points and outliers from each corrected 3D edge feature point; the filtering algorithm includes statistical filtering and voxel grid filtering.

[0019] Preferably, in the extrinsic parameter matrix calculation step, the feature extraction algorithm includes the SURF algorithm and the SIFT algorithm; the feature point matching method is a feature point matching method based on the Superglue neural network; the extrinsic parameter matrix of the laser radar relative to the camera includes a rotation matrix and a translation vector; the nonlinear optimization algorithm includes the EPnP algorithm and the iterative closest point algorithm.

[0020] Preferably, in the data registration and open-loop mileage estimation steps, the geometric features include 3D edge feature point coordinates, normal vectors and curvatures.

[0021] A texture point cloud-based berthing and unberthing self-positioning system for intelligent ships is characterized by comprising a checkerboard image acquisition module, an internal parameter calibration module, an environmental data synchronization acquisition and orthogonal processing module, an external parameter matrix calculation module, a point cloud texture mapping fusion module, a data registration and open-loop mileage estimation module, a closed-loop detection and graph model optimization module, a dock high-precision map construction module, and a real-time self-positioning module connected in sequence.

[0022] The checkerboard image acquisition module uses a fisheye camera to shoot a checkerboard calibration plate pre-set at a fixed position on the dock from different angles to obtain multiple checkerboard images;

[0023] The intrinsic parameter calibration module uses a computer vision algorithm to detect the 2D pixel coordinates of all inner corner points of the checkerboard in each checkerboard image, and establishes 3D world coordinates corresponding to the 2D pixel coordinates of each inner corner point based on the physical size of the checkerboard in each checkerboard image. Based on the 2D pixel coordinates and 3D world coordinates of each inner corner point, the intrinsic parameter matrix and distortion coefficient of the fisheye camera are calculated using the Zhang calibration method to complete the intrinsic parameter calibration of the fisheye camera.

[0024] The environmental data synchronous acquisition and orthogonal processing module uses a laser radar and a calibrated fisheye camera to synchronously acquire environmental data around the dock in real time, obtaining multiple point cloud data collected by the laser radar and multiple environmental images collected by the calibrated fisheye camera; and uses the distortion coefficient to perform orthogonal processing on the environmental image to obtain an orthogonalized environmental image;

[0025] The extrinsic parameter matrix calculation module extracts the coordinates of all 3D edge feature points in the point cloud data in the laser radar coordinate system and the coordinates of all 2D feature points in the orthogonalized environmental image in the camera coordinate system based on the feature extraction algorithm, obtains a point cloud feature set and an environmental image feature set, and uses a feature point matching method to match the 3D edge feature points in the point cloud feature set with the 2D feature points in the environmental image feature set to obtain matched feature point pairs; then uses a RANSAC optimization algorithm to optimize the matched feature point pairs to obtain optimized feature point pairs, and calculates the extrinsic parameter matrix of the laser radar relative to the camera based on the optimized feature point pairs and a nonlinear optimization algorithm;

[0026] The point cloud texture mapping fusion module converts the coordinates of all 3D edge feature points in the point cloud feature set in the laser radar coordinate system into the camera coordinate system through an external parameter matrix, obtains the coordinates of all 3D edge feature points in the camera coordinate system, and projects the coordinates of all 3D edge feature points in the camera coordinate system onto the orthogonalized environment image through an internal parameter matrix to obtain the corresponding projected pixel coordinates; then uses bilinear interpolation to extract the RGB color value of each projected pixel coordinate from the orthogonalized environment image, and assigns the RGB color value to the corresponding 3D edge feature point according to the spatial mapping rule, thereby generating texture point cloud data with color information and texture information;

[0027] The data registration and open-loop mileage estimation module introduces geometric features and RGB color values in texture point cloud data on the basis of the NDT algorithm to construct an improved NDT-color algorithm, and uses the improved NDT-color algorithm to align and register each 3D edge feature point in two adjacent frames of texture point cloud data to obtain the registered texture point cloud data of each frame; then, based on the coordinates of each 3D edge feature point in the registered texture point cloud data, the center coordinates of each frame of the registered texture point cloud data are calculated; based on the center coordinates of the current frame and the frame before the current frame, the displacement vector of the ship in the current frame relative to the previous frame is calculated, and then the displacement vector between each pair of adjacent frames from the starting frame to the current frame is obtained, and then all displacement vectors from the starting frame to the current frame are accumulated to achieve open-loop mileage estimation and form a preliminary position trajectory;

[0028] The closed-loop detection and graph model optimization module uses the texture point cloud data after registration of each frame as a node and the displacement vector between each pair of adjacent frames in the preliminary position trajectory as an edge to construct a graph model; every certain number of frames, it detects whether there is a closed loop among the 3D edge feature points in the registered texture point cloud data; if a closed loop is detected, a closed-loop constraint is added to the graph model, and the number of closed-loop constraints in the graph model is compared with a preset number threshold; if the closed-loop constraints in the graph model are greater than or equal to the preset number threshold, the G2O graph optimization algorithm is used to optimize the graph model to generate an optimized global displacement vector;

[0029] The terminal high-precision map construction module uses the optimized global displacement vector to correct the position of each 3D edge feature point in the texture point cloud data after registration of each frame, thereby obtaining the corrected texture point cloud data of each frame; and generates a terminal high-precision map containing color and texture information based on the corrected texture point cloud data of all frames;

[0030] The real-time self-positioning module compares the point cloud data currently collected by the lidar with the high-precision map of the terminal in real time during the ship's berthing and unberthing process, and uses feature matching algorithm and positioning algorithm to determine the ship's position in the high-precision map of the terminal in real time, thereby realizing real-time self-positioning of the ship in the high-precision map of the terminal.

[0031] Preferably, in the closed-loop detection and graph model optimization module, g2o is first used to construct a graph model representing the geometric constraints and feature matching relationships between point cloud data, with the texture point cloud data after registration of each frame as nodes and the displacement vectors between each pair of adjacent frames in the preliminary position trajectory as edges; a closed-loop detection is started every 20 frames of laser point cloud are collected, and the 3D edge feature points in the registered texture point cloud data are analyzed to determine whether a closed loop exists; when the number of closed-loop constraints in the graph model is greater than or equal to a preset number threshold, the g2o graph optimization algorithm and the NDT-color algorithm are comprehensively used to optimize the graph model, and by adjusting the constraint relationship between nodes in the graph model, the accumulated error is detected and corrected, and the optimized mileage estimate is regenerated, and an optimized global displacement vector is generated based on the optimized mileage estimate.

[0032] Preferably, in the terminal high-precision map construction module, after position correction is performed on each 3D edge feature point, a filtering algorithm is used to remove noise points and outliers from each corrected 3D edge feature point; the filtering algorithm includes statistical filtering and voxel grid filtering.

[0033] Preferably, in the extrinsic parameter matrix calculation module, the feature extraction algorithm includes the SURF algorithm and the SIFT algorithm; the feature point matching method is a feature point matching method based on the Superglue neural network; the extrinsic parameter matrix of the laser radar relative to the camera includes a rotation matrix and a translation vector; the nonlinear optimization algorithm includes the EPnP algorithm and the iterative closest point algorithm.

[0034] Preferably, in the data registration and open-loop mileage estimation module, the geometric features include 3D edge feature point coordinates, normal vectors and curvatures.

[0035] The technical effects of the present invention are as follows:

[0036] The present invention provides a texture point cloud-based berthing and unberthing self-positioning method for intelligent ships. First, a fisheye camera is used to shoot a checkerboard calibration plate from multiple angles to obtain sufficient viewing angle changes to accurately calculate the intrinsic parameter matrix and distortion coefficient of subsequent cameras, which helps to improve the accuracy and robustness of the subsequent calibration process. Computer vision algorithms and Zhang's calibration method are then used to calculate the intrinsic parameter matrix and distortion coefficient of the fisheye camera, ensuring the geometric accuracy of the camera image. The data of the lidar and camera are then collected synchronously in real time. The environmental image is orthogonally processed using the distortion coefficient to obtain the orthogonalized environmental image, which effectively corrects the image deformation caused by lens distortion, improves the geometric accuracy of the image, and provides accurate basic parameters for subsequent data fusion. Then, based on the feature extraction algorithm, the coordinates of all 3D edge feature points in the lidar coordinate system and the coordinates of all 2D feature points in the camera coordinate system are extracted respectively, and the feature point matching method is used to match them to obtain matching feature point pairs. The RANSAC optimization algorithm is then used to optimize the feature point pairs to obtain the optimized feature point pairs. Based on the optimized feature point pairs and the nonlinear optimization algorithm, the extrinsic parameter matrix of the lidar relative to the camera is calculated, which achieves precise alignment between the lidar and camera data, ensures the spatial consistency of the two sensor data, and provides a basis for subsequent data fusion. The coordinates of all 3D edge feature points in the lidar coordinate system are then transformed into the camera coordinate system using the external parameter matrix, and the coordinates of all 3D edge feature points in the camera coordinate system are projected onto the orthogonalized environment image using the internal parameter matrix to obtain the corresponding projected pixel coordinates. The RGB color value of each projected pixel coordinate is then extracted from the orthogonalized environment image using the bilinear interpolation method, and the RGB color value is assigned to the corresponding 3D edge feature point according to the spatial mapping rule, thereby generating textured point cloud data with color and texture information. By using the camera to assign color texture to the lidar point cloud data, that is, mapping the color texture information in the camera image to the lidar point cloud data, the conversion from the lidar coordinate system to the camera coordinate system and then to the image pixel coordinate system is essentially a two-sensor plane coordinate transformation. Based on this, the lidar point cloud data can be associated with the camera image data, and the three-dimensional spatial information of the lidar and the color and texture information of the camera can be organically combined to achieve point cloud texture mapping data fusion, which greatly improves the visualization of the point cloud data and makes it intuitive and easy to understand. It is easy for crew members to intuitively and quickly identify various targets, greatly shortening decision-making time and improving the efficiency and safety of berthing and unberthing operations.Then, based on the NDT algorithm, the geometric features and RGB color values in the texture point cloud data are introduced to construct an improved NDT-color algorithm. By combining geometric features and color information, the accuracy and stability of the algorithm feature matching are enhanced, making the registration results more reliable; and the improved NDT-color algorithm is used to align the texture point cloud data, which greatly improves the registration accuracy and robustness of the texture point cloud data, ensures the precise alignment of the point cloud data of adjacent frames in the global coordinate system, and provides high-quality basic data for subsequent open-loop mileage estimation and map construction. The open-loop mileage estimation calculates the displacement change of the ship between adjacent frames, does not rely on loop detection, and the calculation is relatively simple. It can quickly obtain the result of the ship's position change, meeting the real-time requirements of the ship's berthing and leaving. By providing a preliminary determination of the ship's position change in real time, it can provide the crew with approximate information on the ship's real-time position change, so that they can have a basic judgment on the ship's motion trend; then, the texture point cloud data after each frame of alignment is used as a node, and the displacement vector between each pair of adjacent frames in the preliminary position trajectory is used as an edge to construct a graph model, and the aligned texture point cloud data is closed-loop detected to identify Whether there is the same physical position between the current frame and the historical frame to reduce the cumulative error and improve global consistency; if a closed loop is detected, a closed-loop constraint is added to the graph model to enhance global consistency and effectively detect and correct the accumulated error. The closed-loop constraint helps to reduce the cumulative error and improve the overall quality of the trajectory; and the G2O graph optimization algorithm is used to optimize the graph model to generate an optimized global displacement vector, which effectively improves the accuracy and consistency of the trajectory; then the optimized global displacement vector is used to correct the position of each 3D edge feature point in the texture point cloud data after each frame registration to ensure that the position of each frame of point cloud data in the global coordinate system is accurate; then a high-precision map of the terminal containing color texture information is generated, which can accurately reflect the actual situation of the terminal. Finally, the point cloud data currently collected by the lidar is compared with the high-precision map of the terminal in real time, and the feature matching and positioning algorithm is used to determine the position of the ship in the high-precision map of the terminal in real time, realizing the real-time self-positioning of the ship in the high-precision map of the terminal, providing accurate position information for the ship's navigation system, supporting automated berthing and unberthing operations, and improving the safety and efficiency of operations. The present invention constructs a complete SLAM algorithm system, which provides stable and accurate positioning and navigation for ships in all aspects, from data collection, alignment, open-loop mileage estimation, closed-loop detection and optimization, to map construction, meeting the needs of the entire process of complex terminal operations.

[0037] The present invention abandons the complex mode of drone assistance and relies only on ship-borne lidar and cameras. The system is simple and stable, not restricted by additional factors such as external airspace and weather, and can be put into use at any time, effectively reducing operation and maintenance costs and operational difficulty, and ensuring the normalization and reliability of ship berthing and unberthing operations. By shooting the checkerboard calibration plate from multiple angles and using Zhang's calibration method to calculate the internal parameter matrix and distortion coefficient of the fisheye camera, the geometric accuracy of the camera image is ensured; the data of the lidar and camera are collected synchronously in real time, and the camera image is dedistorted to ensure the consistency and accuracy of the data; the external parameter matrix between the lidar and the camera is calculated through feature extraction and matching algorithms to achieve precise alignment between the two; further, texture point cloud data with color information is generated through data fusion, making the point cloud data intuitive and easy to understand, and the crew can quickly identify the target, greatly shortening the decision-making time, and improving the efficiency and safety of berthing and unberthing operations; high-precision point cloud registration and displacement calculation are achieved through the improved NDT-color algorithm, and the accumulated errors of registration and displacement are effectively corrected using closed-loop detection and graph optimization technology, and in After the closed loop is detected, a new edge (closed-loop constraint) is added to the graph model to directly connect the current frame node with the matching historical frame node. This closed-loop constraint provides additional global consistency information for the graph model to correct the drift error in the trajectory. The global displacement vector after graph optimization can significantly reduce the drift error in the preliminary position trajectory and generate a more accurate global trajectory. The corrected global trajectory not only improves the accuracy of map construction, but also provides a more reliable foundation for real-time self-positioning. Finally, a high-quality high-precision map of the terminal is generated. On this basis, real-time and accurate ship self-positioning can be achieved, supporting automated berthing and unberthing operations, significantly improving the safety, efficiency and robustness of the operation, and providing an efficient, accurate and reliable solution to ensure the efficient and safe operation of ships in complex environments.

[0038] The present invention also relates to a berthing and unberthing self-positioning system based on texture point cloud in an intelligent ship. The system corresponds to the above-mentioned berthing and unberthing self-positioning method based on texture point cloud in an intelligent ship, and can be understood as a system that realizes the above-mentioned berthing and unberthing self-positioning method based on texture point cloud in an intelligent ship, comprising a checkerboard image acquisition module, an internal parameter calibration module, an environmental data synchronous acquisition and orthogonal processing module, an external parameter matrix calculation module, a point cloud texture mapping fusion module, a data alignment and open-loop mileage estimation module, a closed-loop detection and graph model optimization module, a terminal high-precision map construction module and a real-time self-positioning module connected in sequence. The modules work together to ensure the geometric accuracy of the camera image by shooting the checkerboard calibration plate from multiple angles and using Zhang's calibration method to calculate the internal parameter matrix and distortion coefficient of the fisheye camera; collect data from the lidar and camera in real time and synchronously, and perform orthogonal processing on the camera image. The system ensures data consistency and accuracy; calculates the extrinsic parameter matrix between the lidar and the camera through feature extraction and matching algorithms, and achieves precise alignment between the two; further generates textured point cloud data with color and texture information through point cloud texture mapping data fusion, making the point cloud data intuitive and easy to understand, allowing crew members to quickly identify targets, greatly shortening decision-making time, and improving the efficiency and safety of berthing and unberthing operations; achieves high-precision point cloud registration and open-loop mileage estimation through the improved NDT-color algorithm, and effectively corrects accumulated errors using closed-loop detection and graph optimization technology to generate high-quality high-precision maps of the terminal. On this basis, it can achieve real-time and accurate ship self-positioning, support automated berthing and unberthing operations, significantly improve operational safety, efficiency and system robustness, and provide an efficient, accurate and reliable solution to ensure the efficient and safe operation of ships in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 The present invention is a flow chart of a method for self-positioning of docking and undocking based on texture point cloud in an intelligent ship.

[0040] Figure 2 This is a schematic diagram of the high-precision map of the dock containing color texture information of the present invention.

[0041] Figure 3 This is a schematic diagram of the track prediction of a ship in the present invention during real-time self-positioning in a high-precision map of a dock. DETAILED DESCRIPTION

[0042] The present invention will be described below with reference to the accompanying drawings.

[0043] The present invention relates to a method for self-positioning of berthing and unberthing in an intelligent ship based on texture point cloud. The flow chart of the method is as follows: Figure 1 As shown, the following steps are included in sequence:

[0044] 1. Checkerboard Image Capture Step: Use a fisheye camera to capture multiple checkerboard images from different angles of a checkerboard calibration plate pre-installed at a fixed location on the dock. This step uses the checkerboard calibration plate to create a calibration scene, ensuring that the checkerboard is within the fisheye camera's field of view and that the image is clear and complete.

[0045] Specifically, a high-precision checkerboard calibration plate (e.g., 1m×1m in size, 10cm±0.05mm in square side length) is first installed at a fixed position on the dock to ensure that its flatness error is less than 0.1mm / m. The calibration plate is made of matte black and white with a reflectivity difference of more than 80% (to avoid interference from the laser radar intensity). A fisheye camera is then used to photograph the checkerboard calibration plate from different angles (usually 15-20 sets of different postures are required, and each posture ensures that the calibration plate appears completely in the image) to obtain multiple checkerboard images.

[0046] 2. Intrinsic parameter calibration step: Use computer vision algorithm to detect the 2D pixel coordinates of all inner corner points of the checkerboard in each checkerboard image. At the same time, establish the 3D world coordinates corresponding to the 2D pixel coordinates of each inner corner point according to the physical size of the checkerboard in each checkerboard image. Based on the 2D pixel coordinates and 3D world coordinates of each inner corner point, and use Zhang's calibration method to calculate the intrinsic parameter matrix and distortion coefficient of the fisheye camera, the intrinsic parameter calibration of the fisheye camera is completed.

[0047] Specifically, a computer vision algorithm is first used to detect the 2D pixel coordinates (u, v) of all inner corner points of the checkerboard in each checkerboard image. At the same time, based on the physical size of the checkerboard in each checkerboard image (e.g., each square is 10 cm × 10 cm), the 3D world coordinates (X, Y, 0) corresponding to the 2D pixel coordinates of each inner corner point are established, where Z = 0 because the checkerboard is a plane. Then, based on the 2D pixel coordinates (u, v) and 3D world coordinates (X, Y, 0) of each inner corner point, Zhang's calibration method (preferably the Scaramuzza model) is used for internal calibration to calculate the fisheye camera's internal parameter matrix and radial distortion coefficient, completing the fisheye camera's internal parameter calibration. The radial distortion can be described by a polynomial model, and a cubic polynomial is commonly used to fit the radial distortion:

[0048]

[0049] In the above formula, r undistorted is the corrected radial distance, and K1, K2, and K3 are the radial distortion coefficients.

[0050] Among them, in actual application scenarios, the industry usually uses fisheye lenses to obtain a larger field of view. By calibrating the fisheye camera using a checkerboard grid, the intrinsic parameter expression of its distance projection model can be obtained:

[0051] [rd =f·θ] (2)

[0052] Among them, r d is the radial distance on the image plane (the distance from the principal point to the image point), f is the equivalent focal length of the fisheye lens, and θ is the angle between the light ray and the optical axis (the angle of incidence).

[0053] For the intrinsic parameter matrix of the fisheye camera, it is usually expressed as K. In the pinhole camera model, the general form of the intrinsic parameter matrix is:

[0054]

[0055] In the case of fisheye cameras, due to their special imaging characteristics, f x and f y It is related to the equivalent focal length f and the radial distortion parameter of the fisheye camera. And, c x and c y Represent the coordinates of the principal point of the image in the x and y directions respectively.

[0056] In summary, in practical applications, the intrinsic parameters of a fisheye camera need to consider multiple parameters such as equivalent focal length, principal point coordinates, and radial distortion coefficient. These parameters together determine how the fisheye camera projects points in three-dimensional space onto a two-dimensional image plane. By correcting radial distortion, a more accurate image geometry can be obtained.

[0057] 3. Synchronous collection and orthogonal processing of environmental data: The environmental data around the dock is collected synchronously in real time by using the LiDAR and a calibrated fisheye camera. Multiple point cloud data collected by the LiDAR and multiple environmental images collected by the calibrated fisheye camera are obtained respectively. The environmental image is orthogonally processed using the distortion coefficient to obtain an orthogonalized environmental image (i.e., an orthogonal image) for subsequent more accurate fusion with the LiDAR point cloud.

[0058] The laser radar and fisheye camera are triggered synchronously to collect data. Specifically, the laser radar and the calibrated fisheye camera realize real-time synchronous collection of environmental data around the terminal through clock synchronization. This makes it possible that in the subsequent data fusion step, the 3D edge feature points in the point cloud feature set and the 2D feature points in the environmental image feature set have a higher degree of matching in the time dimension, reducing the feature matching error caused by time asynchrony, improving the accuracy and reliability of the fused point cloud data, and thus improving the accuracy and efficiency of key target identification at the terminal.

[0059] Furthermore, a high-precision pulse synchronization signal generator can be used to synchronize the clocks of the lidar and the calibrated fisheye camera, thereby synchronously collecting environmental data around the dock in real time.

[0060] 4. Calculation steps of the external parameter matrix: Based on the feature extraction algorithm (such as SURF algorithm or SIFT algorithm), the coordinates of all 3D edge feature points in the point cloud data in the laser radar coordinate system and the coordinates of all 2D feature points in the orthogonalized environmental image in the camera coordinate system are extracted respectively, and the point cloud feature set and the environmental image feature set are obtained respectively. The 3D edge feature points in the point cloud feature set are matched with the 2D feature points in the environmental image feature set by the feature point matching method based on the Superglue neural network to obtain matched feature point pairs; the matched feature point pairs are optimized by the RANSAC optimization algorithm to obtain optimized feature point pairs; the external parameter matrix of the laser radar relative to the camera is calculated based on the optimized feature point pairs and a nonlinear optimization algorithm (such as EPnP algorithm or iterative nearest point algorithm). That is, once the matching feature point pairs are found, these matching points can be used to optimize the external parameters (rotation matrix R wl and the translation vector t wl This step uses an optimization algorithm based on feature point matching, such as the improved iterative closest point algorithm and a feature point matching method based on the Superglue neural network and iterative optimization, to match the lidar point cloud features with the camera image features, accurately estimate the extrinsic parameters between the lidar and camera, and determine the precise spatial relative relationship between the two, namely the relative position and posture transformation relationship.

[0061] Let the matching feature point pair be (P ci , P li ), then the optimization objective function can be defined as:

[0062]

[0063] By minimizing the objective function, for example, by using a nonlinear optimization algorithm (such as the EPnP algorithm, the iterative closest point algorithm, or the Levenberg-Marquardt algorithm) to calculate the extrinsic parameter matrix of the LiDAR relative to the camera, a more accurate extrinsic parameter estimate can be obtained. Preferably, the extrinsic parameter matrix of the LiDAR relative to the camera includes a rotation matrix and a translation vector.

[0064] 5. Point cloud texture mapping fusion steps: The coordinates of all 3D edge feature points in the point cloud feature set in the lidar coordinate system are converted to the camera coordinate system through the external parameter matrix to obtain the coordinates of all 3D edge feature points in the camera coordinate system, and the coordinates of all 3D edge feature points in the camera coordinate system are projected onto the orthogonalized environmental image through the internal parameter matrix to obtain the corresponding projected pixel coordinates; then the bilinear interpolation method is used to extract the RGB color value of each projected pixel coordinate from the orthogonalized environmental image, and the RGB color value is assigned to the corresponding 3D edge feature point according to the spatial mapping rule, thereby generating texture point cloud data with color information and texture information.

[0065] Specifically, the coordinate system is defined first: the camera coordinate system O c -X c Y c Z c Taking the optical center of the camera as the origin, Z c The axis coincides with the camera optical axis, X c and Y c The axes constitute the horizontal and vertical axes of the image plane. w -X w Y w Z w ) is a global reference frame used to describe the positions of objects in the environment.

[0066] Assume that the external parameter matrix from the camera coordinate system to the world coordinate system, that is, the rotation matrix is R cw , the translation vector is t cw , then the coordinate relationship of a point F in space in the camera coordinate system and the world coordinate system is:

[0067] P w =R cw P c +t cw (4)

[0068] Among them, P w is the coordinate of point F in the world coordinate system, P c is the coordinate of point F in the camera coordinate system (also called the camera coordinate system).

[0069] LiDAR coordinate system O l -X l Y l Z l The origin is usually located at the center of the laser radar, and the coordinate axis is defined according to the installation direction of the laser radar. Assume that the rotation matrix from the laser radar coordinate system to the world coordinate system is R lw , the translation vector is t lw , then the coordinate transformation relationship between point F in the lidar coordinate system and the world coordinate system is:

[0070] P w =R lw P l +t lw (5)

[0071] Among them, P l is the coordinate of point F in the lidar coordinate system.

[0072] From the above relationship, the conversion relationship between the camera coordinate system and the lidar coordinate system is:

[0073] P l =R wl (P c -t cw )+t lw (6)

[0074] in, is the rotation matrix from the lidar coordinate system to the camera coordinate system, t wl =-R wl t cw +t lw is the translation vector from the lidar coordinate system to the camera coordinate system.

[0075] Based on the conversion relationship between the camera coordinate system and the lidar coordinate system, the coordinates of all 3D edge feature points in the point cloud feature set located in the lidar coordinate system can be converted to the camera coordinate system through the external parameter matrix, obtaining the coordinates of all 3D edge feature points in the camera coordinate system. The coordinates of all 3D edge feature points in the camera coordinate system are then projected onto the dedistorted environment image through the internal parameter matrix to obtain the corresponding projected pixel coordinates, thus achieving the forward mapping from the lidar point cloud space to the camera image space. The bilinear interpolation method is then used to extract the RGB color value of each projected pixel coordinate from the dedistorted environment image, and the RGB color value is assigned to the corresponding 3D edge feature point according to the spatial mapping rule, achieving the reverse inverse mapping, that is, mapping the color and texture information in the camera image to the lidar point cloud data, thereby generating textured point cloud data with color and texture information. By using the internal and external parameter matrices, the conversion of point cloud coordinates and the mapping of color and texture information are achieved, so that the fused data contains rich spatial and visual information. The fused data, i.e. the generated texture point cloud data with color information and texture information, can also be expressed in a unified coordinate system (such as the world coordinate system). For example, for the lidar point cloud data, it is converted to the world coordinate system through coordinate transformation, and each point is given the color information of the corresponding position in the camera image (i.e. the dedistorted environment image) (if feature matching and color mapping are performed). Let the fused point cloud data be P f ,but:

[0076] P f =R lw P l +t lw (7)

[0077] Moreover, P f The color information can be obtained from the camera image based on feature matching.

[0078] This data fusion step is based on the calibrated camera internal parameters and the accurately obtained external parameters. Through the plane coordinate transformation and interpolation algorithm of the two sensors, the point cloud is mapped to the camera image, and then the reverse inverse mapping is completed. The color values corresponding to the camera image pixels are assigned to the lidar point cloud according to the spatial mapping rules to generate fused point cloud data rich in color texture and strong visualization. That is, the color texture information in the camera image is mapped to the lidar point cloud, which greatly improves the visualization of the point cloud data and makes it easier for crew members to intuitively identify various targets.

[0079] VI. Data Registration and Open-Loop Odometer Estimation Steps: Based on the NDT algorithm, the geometric features and RGB color values of the texture point cloud data are introduced to construct an improved NDT-color algorithm. The improved NDT-color algorithm is then used to align and register the 3D edge feature points in two adjacent frames of texture point cloud data, obtaining the registered texture point cloud data for each frame. The center coordinates of each frame of the registered texture point cloud data are then calculated based on the coordinates of the 3D edge feature points in the registered texture point cloud data. The displacement vector of the ship in the current frame relative to the previous frame is calculated based on the center coordinates of the current frame and the previous frame, thereby obtaining the displacement vector between each pair of adjacent frames from the starting frame to the current frame. All displacement vectors from the starting frame to the current frame are then accumulated to achieve open-loop odometer estimation and form a preliminary position trajectory. This step introduces the improved NDT-color algorithm to register the texture point cloud, fully utilizing color information to improve registration accuracy. On this basis, open-loop odometer estimation is achieved, and the position changes of the ship during the berthing and unberthing process are preliminarily determined.

[0080] Specifically, first, on the basis of the NDT algorithm, the geometric features and RGB color values in the texture point cloud data are introduced to construct an improved NDT-color algorithm, and the improved NDT-color algorithm is used to align and register the 3D edge feature points in two adjacent frames of texture point cloud data to obtain the texture point cloud data after registration of each frame, that is, the NDT-color algorithm is used to calculate the best matching position between the two frames of texture point clouds, which not only considers the geometric features (that is, the spatial position of the points, such as the 3D edge feature point coordinates, normal vectors and curvature), but also combines the color information (RGB values) to increase the accuracy of matching.

[0081] Then, the center coordinates of each frame of registered texture point cloud data are calculated based on the coordinates of each 3D edge feature point in the registered texture point cloud data; that is, for each frame of point cloud data, its center coordinates (center of mass) are calculated first. Assume that the texture point cloud data of the i-th frame after registration is P i ={P i，1 , P i，2 ,...P i，n}, where each 3D edge feature point P ij Is a three-dimensional coordinate vector (x, y, z). Then the center coordinate C of the texture point cloud data after the frame registration is i It can be calculated by the following formula:

[0082]

[0083] Where n is the number of points in the texture point cloud data after registration of the frame.

[0084] Then, the displacement vector of the ship in the current frame relative to the previous frame is calculated based on the center coordinates of the current frame and the previous frame. That is, the displacement between two adjacent frames is calculated based on the change of the point cloud center. Assuming that the current frame is i and the previous frame is i-1, the displacement vector ΔT between the two frames is i，i-1 It can be expressed as:

[0085] ΔT i，i-1 =C i -C i-1 (9)

[0086] This displacement vector contains the distance and direction of the ship's movement between the two frames. Then, the displacement vector between each pair of adjacent frames from the starting frame to the current frame is obtained.

[0087] Finally, all displacement vectors from the starting frame to the current frame are accumulated to realize open-loop mileage estimation and form a preliminary position trajectory. That is, in order to obtain the position trajectory of the ship in the entire sequence, the displacement vectors between each pair of adjacent frames need to be accumulated. The open-loop mileage estimation only focuses on the relationship between the current frame and the previous frame. By analyzing the changes in the 3D edge feature points in the point cloud data of these two frames, the position change of the ship is determined. There is no need to use loop detection to correct the accumulated error, which reduces the complexity of data processing and improves the computational efficiency. It can quickly obtain the result of the ship's position change and meet the real-time requirements during the ship's berthing and unberthing process. Assume that the initial position is T0 = (0, 0, 0), then the cumulative displacement vector T at the i-th frame is i It can be calculated by the following recursive formula:

[0088] T i =T i-1 +ΔT i，i-1 (10)

[0089] Among them, T i-1 is the cumulative displacement vector of the previous frame.

[0090] The final output is a preliminary, unoptimized position trajectory {T0, T1, T2...TN}, where N is the total number of frames. This trajectory is based on the direct accumulation of point cloud registration results and may have cumulative errors, but it can provide a preliminary position estimate.

[0091] 7. Closed loop detection and graph model optimization steps: Use the texture point cloud data after each frame registration as the node and the displacement vector between each pair of adjacent frames in the preliminary position trajectory as the edge to construct the graph model; every certain number of frames, detect whether there is a closed loop in the 3D edge feature points in the registered texture point cloud data, that is, every 20 frames, trigger a closed loop detection to detect whether there is a closed loop in the 3D edge feature points in the registered texture point cloud data (that is, check the possibility of whether the current frame and the previous frame have the same physical position). If a closed loop is detected (that is, the current frame corresponds to the same physical position as a historical frame), add a closed loop constraint to the graph model (add a new edge to the graph model, that is, the edge connecting the current frame and the historical frame), and compare the number of closed loop constraints in the graph model with the preset number threshold (such as 5). If the closed loop constraints in the graph model are greater than or equal to the preset number threshold (such as 5), the g2o graph optimization algorithm is used to optimize the graph model to generate an optimized global displacement vector.

[0092] Furthermore, for constructing a graph model, g2o can be used first, with the texture point cloud data after each frame registration as the node and the displacement vector between each pair of adjacent frames in the preliminary position trajectory as the edge, to construct a graph model representing the geometric constraints and feature matching relationships between the point cloud data; every 20 frames of laser point cloud are collected, a closed-loop detection is started, and the 3D edge feature points in the registered texture point cloud data are analyzed to determine whether there is a closed loop; when the number of closed-loop constraints in the graph model is greater than or equal to the preset number threshold, the g2o graph optimization algorithm and the NDT-color algorithm are comprehensively used to optimize the graph model, and by adjusting the constraint relationship between the nodes in the graph model, the accumulated error is detected and corrected, and the optimized mileage estimate is regenerated, and the optimized global displacement vector is generated based on the optimized mileage estimate.

[0093] 8. Steps for constructing high-precision maps of the terminal: Use the optimized global displacement vector to correct the position of each 3D edge feature point in the texture point cloud data after registration of each frame to obtain the corrected texture point cloud data of each frame; Based on the texture point cloud data after correction of all frames, a high-precision map of the terminal containing color texture information is generated. The high-precision map of the terminal is as follows: Figure 2 shown.

[0094] Furthermore, after position correction is performed on each 3D edge feature point, a filtering algorithm is used to remove noise points and outliers from each corrected 3D edge feature point; the filtering algorithm includes statistical filtering and voxel grid filtering.

[0095] 9. Real-time self-positioning steps: During the berthing and unberthing process, the point cloud data currently collected by the LiDAR is compared with the high-precision map of the terminal in real time, and the feature matching algorithm (such as nearest neighbor search, FLANN, etc.) and positioning algorithm (such as EKF, particle filter, etc.) are used to determine the position of the ship in the high-precision map of the terminal in real time, so as to achieve real-time self-positioning of the ship in the high-precision map of the terminal, thereby providing accurate navigation information for the berthing and unberthing operations of the ship. The track prediction of the ship's real-time self-positioning in the high-precision map of the terminal is as follows: Figure 3 shown.

[0096] The present invention also relates to a berthing and unberthing self-positioning system based on texture point cloud in an intelligent ship. The system corresponds to the berthing and unberthing self-positioning method based on texture point cloud in the above-mentioned intelligent ship, and can be understood as a system for implementing the above-mentioned method. The system includes a checkerboard image acquisition module, an internal parameter calibration module, an environmental data synchronization acquisition and orthogonal processing module, an external parameter matrix calculation module, a point cloud texture mapping fusion module, a data registration and open-loop mileage estimation module, a closed-loop detection and graph model optimization module, a terminal high-precision map construction module and a real-time self-positioning module connected in sequence. Specifically,

[0097] The checkerboard image acquisition module uses a fisheye camera to shoot a checkerboard calibration plate pre-set at a fixed position on the dock from different angles to obtain multiple checkerboard images;

[0098] The intrinsic parameter calibration module uses a computer vision algorithm to detect the 2D pixel coordinates of all inner corner points of the checkerboard in each checkerboard image, and establishes 3D world coordinates corresponding to the 2D pixel coordinates of each inner corner point based on the physical size of the checkerboard in each checkerboard image. Based on the 2D pixel coordinates and 3D world coordinates of each inner corner point, the intrinsic parameter matrix and distortion coefficient of the fisheye camera are calculated using the Zhang calibration method to complete the intrinsic parameter calibration of the fisheye camera.

[0099] The environmental data synchronous acquisition and orthogonal processing module uses a laser radar and a calibrated fisheye camera to synchronously acquire environmental data around the dock in real time, obtaining multiple point cloud data collected by the laser radar and multiple environmental images collected by the calibrated fisheye camera; and uses the distortion coefficient to perform orthogonal processing on the environmental image to obtain an orthogonalized environmental image;

[0100] The extrinsic parameter matrix calculation module extracts the coordinates of all 3D edge feature points in the point cloud data in the laser radar coordinate system and the coordinates of all 2D feature points in the orthogonalized environmental image in the camera coordinate system based on the feature extraction algorithm, obtains a point cloud feature set and an environmental image feature set, and uses a feature point matching method to match the 3D edge feature points in the point cloud feature set with the 2D feature points in the environmental image feature set to obtain matched feature point pairs; then uses a RANSAC optimization algorithm to optimize the matched feature point pairs to obtain optimized feature point pairs, and calculates the extrinsic parameter matrix of the laser radar relative to the camera based on the optimized feature point pairs and a nonlinear optimization algorithm;

[0101] The point cloud texture mapping fusion module converts the coordinates of all 3D edge feature points in the point cloud feature set in the laser radar coordinate system into the camera coordinate system through an external parameter matrix, obtains the coordinates of all 3D edge feature points in the camera coordinate system, and projects the coordinates of all 3D edge feature points in the camera coordinate system onto the orthogonalized environment image through an internal parameter matrix to obtain the corresponding projected pixel coordinates; then uses bilinear interpolation to extract the RGB color value of each projected pixel coordinate from the orthogonalized environment image, and assigns the RGB color value to the corresponding 3D edge feature point according to the spatial mapping rule, thereby generating texture point cloud data with color information and texture information;

[0102] The data registration and open-loop mileage estimation module introduces geometric features and RGB color values in texture point cloud data on the basis of the NDT algorithm to construct an improved NDT-color algorithm, and uses the improved NDT-color algorithm to align and register each 3D edge feature point in two adjacent frames of texture point cloud data to obtain the registered texture point cloud data of each frame; then, based on the coordinates of each 3D edge feature point in the registered texture point cloud data, the center coordinates of each frame of the registered texture point cloud data are calculated; based on the center coordinates of the current frame and the frame before the current frame, the displacement vector of the ship in the current frame relative to the previous frame is calculated, and then the displacement vector between each pair of adjacent frames from the starting frame to the current frame is obtained, and then all displacement vectors from the starting frame to the current frame are accumulated to achieve open-loop mileage estimation and form a preliminary position trajectory;

[0103] The closed-loop detection and graph model optimization module uses the texture point cloud data after registration of each frame as a node and the displacement vector between each pair of adjacent frames in the preliminary position trajectory as an edge to construct a graph model; every certain number of frames, it detects whether there is a closed loop among the 3D edge feature points in the registered texture point cloud data; if a closed loop is detected, a closed-loop constraint is added to the graph model, and the number of closed-loop constraints in the graph model is compared with a preset number threshold; if the closed-loop constraints in the graph model are greater than or equal to the preset number threshold, the G2O graph optimization algorithm is used to optimize the graph model to generate an optimized global displacement vector;

[0104] The terminal high-precision map construction module uses the optimized global displacement vector to correct the position of each 3D edge feature point in the texture point cloud data after registration of each frame, thereby obtaining the corrected texture point cloud data of each frame; and generates a terminal high-precision map containing color and texture information based on the corrected texture point cloud data of all frames;

[0105] The real-time self-positioning module compares the point cloud data currently collected by the lidar with the high-precision map of the terminal in real time during the ship's berthing and unberthing process, and uses feature matching algorithm and positioning algorithm to determine the ship's position in the high-precision map of the terminal in real time, thereby realizing real-time self-positioning of the ship in the high-precision map of the terminal.

[0106] Preferably, in the closed-loop detection and graph model optimization module, g2o is first used to construct a graph model representing the geometric constraints and feature matching relationships between point cloud data, with the texture point cloud data after registration of each frame as nodes and the displacement vectors between each pair of adjacent frames in the preliminary position trajectory as edges; a closed-loop detection is started every 20 frames of laser point cloud are collected, and the 3D edge feature points in the registered texture point cloud data are analyzed to determine whether a closed loop exists; when the number of closed-loop constraints in the graph model is greater than or equal to a preset number threshold, the g2o graph optimization algorithm and the NDT-color algorithm are comprehensively used to optimize the graph model, and by adjusting the constraint relationship between nodes in the graph model, the accumulated error is detected and corrected, and the optimized mileage estimate is regenerated, and an optimized global displacement vector is generated based on the optimized mileage estimate.

[0107] Preferably, in the high-precision map construction module of the terminal, after position correction is performed on each 3D edge feature point, a filtering algorithm is used to remove noise points and outliers from each corrected 3D edge feature point; the filtering algorithm includes statistical filtering and voxel grid filtering.

[0108] Preferably, in the extrinsic parameter matrix calculation module, the feature extraction algorithm includes a SURF algorithm and a SIFT algorithm; and the feature point matching method is a feature point matching method based on a Superglue neural network.

[0109] Preferably, in the extrinsic parameter matrix calculation module, the extrinsic parameter matrix of the laser radar relative to the camera includes a rotation matrix and a translation vector; and the nonlinear optimization algorithm includes an EPnP algorithm and an iterative closest point algorithm.

[0110] Preferably, in the data registration and open-loop mileage estimation module, the geometric features include 3D edge feature point coordinates, normal vectors and curvatures.

[0111] The present invention provides an objective and scientific method and system for berthing and unberthing self-positioning in intelligent ships based on texture point cloud. By shooting a checkerboard calibration plate from multiple angles and using Zhang's calibration method to calculate the internal parameter matrix and distortion coefficient of the fisheye camera, the geometric accuracy of the camera image is ensured. The data of the laser radar and the camera are collected synchronously in real time, and the camera image is orthogonally processed to ensure the consistency and accuracy of the data. The external parameter matrix between the laser radar and the camera is calculated through feature extraction and matching algorithm to achieve precise alignment of the two. The three-dimensional spatial information of the laser radar is further organically combined with the color texture information of the camera through forward mapping and reverse inverse mapping. The point cloud texture is obtained by the laser radar and the camera. Mapping fusion generates textured point cloud data with color information, making the point cloud data intuitive and easy to understand. Crew members can quickly identify targets, greatly shortening decision-making time and improving the efficiency and safety of berthing and unberthing operations. Through the improved NDT-color algorithm, high-precision point cloud registration and open-loop mileage estimation are achieved, and closed-loop detection and graph optimization technology are used to effectively correct accumulated errors and generate high-quality high-precision maps of the terminal. On this basis, real-time and accurate ship self-positioning can be achieved, supporting automated berthing and unberthing operations, significantly improving operational safety, efficiency and system robustness, and providing an efficient, accurate and reliable solution to ensure the efficient and safe operation of ships in complex environments.

[0112] It should be noted that the specific embodiments described above can enable those skilled in the art to more fully understand the present invention, but do not limit the present invention in any way. Therefore, although this specification has described the present invention in detail with reference to the drawings and embodiments, those skilled in the art should understand that the present invention can still be modified or replaced with equivalents. In short, all technical solutions and improvements that do not depart from the spirit and scope of the present invention should be included in the scope of protection of the patent for the present invention.

Claims

1. A method for berthing and unberthing self-positioning in an intelligent ship based on texture point cloud, characterized in that: The following steps are involved: Checkerboard image acquisition steps: Use a fisheye camera to shoot a checkerboard calibration plate pre-set at a fixed position on the dock from different angles to obtain multiple checkerboard images; Intrinsic calibration: A computer vision algorithm is used to detect the 2D pixel coordinates of all inner corner points of each checkerboard grid in each checkerboard grid image. The 3D world coordinates corresponding to the 2D pixel coordinates of each inner corner point are then established based on the physical dimensions of the checkerboard grid in each checkerboard grid image. Based on the 2D pixel coordinates and 3D world coordinates of each inner corner point, the Zhang calibration method is used to calculate the intrinsic parameter matrix and distortion coefficient of the fisheye camera, completing the intrinsic calibration of the fisheye camera. Environmental data synchronous collection and orthogonal processing steps: The environmental data around the dock is collected synchronously and in real time using a LiDAR and a calibrated fisheye camera. Multiple point cloud data collected by the LiDAR and multiple environmental images collected by the calibrated fisheye camera are obtained. The environmental images are then orthogonally processed using distortion coefficients to obtain the orthogonalized environmental images. The steps for calculating the extrinsic parameter matrix are as follows: based on the feature extraction algorithm, the coordinates of all 3D edge feature points in the point cloud data in the laser radar coordinate system and the coordinates of all 2D feature points in the orthogonalized environment image in the camera coordinate system are extracted respectively, and the point cloud feature set and the environment image feature set are obtained respectively. The 3D edge feature points in the point cloud feature set are matched with the 2D feature points in the environment image feature set using the feature point matching method to obtain matched feature point pairs; the matched feature point pairs are then optimized using the RANSAC optimization algorithm to obtain optimized feature point pairs, and the extrinsic parameter matrix of the laser radar relative to the camera is calculated based on the optimized feature point pairs and a nonlinear optimization algorithm; Point cloud texture mapping fusion steps: The coordinates of all 3D edge feature points in the point cloud feature set in the lidar coordinate system are converted to the camera coordinate system through the external parameter matrix to obtain the coordinates of all 3D edge feature points in the camera coordinate system, and the coordinates of all 3D edge feature points in the camera coordinate system are projected onto the orthogonalized environment image through the internal parameter matrix to obtain the corresponding projected pixel coordinates; the RGB color value of each projected pixel coordinate is extracted from the orthogonalized environment image using the bilinear interpolation method, and the RGB color value is assigned to the corresponding 3D edge feature point according to the spatial mapping rule, thereby generating texture point cloud data with color information and texture information; Data registration and open-loop mileage estimation steps: Based on the NDT algorithm, the geometric features and RGB color values in the texture point cloud data are introduced to construct an improved NDT-color algorithm, and the improved NDT-color algorithm is used to align and register each 3D edge feature point in the texture point cloud data of two adjacent frames to obtain the registered texture point cloud data of each frame; then, based on the coordinates of each 3D edge feature point in the registered texture point cloud data, the center coordinates of each frame of the registered texture point cloud data are calculated; according to the center coordinates of the current frame and the previous frame of the current frame, the displacement vector of the ship in the current frame relative to the previous frame is calculated, and then the displacement vector between each pair of adjacent frames from the starting frame to the current frame is obtained, and then all displacement vectors from the starting frame to the current frame are accumulated to achieve open-loop mileage estimation and form a preliminary position trajectory; Closed-loop detection and graph model optimization steps: Build a graph model using the texture point cloud data after registration of each frame as nodes and the displacement vectors between each pair of adjacent frames in the preliminary position trajectory as edges; Every certain number of frames, the 3D edge feature points in the registered texture point cloud data are detected to see if there is a closed loop. If a closed loop is detected, a closed loop constraint is added to the graph model, and the number of closed loop constraints in the graph model is compared with a preset number threshold. If the closed loop constraints in the graph model are greater than or equal to the preset number threshold, the G2O graph optimization algorithm is used to optimize the graph model to generate an optimized global displacement vector. Steps for constructing a high-precision map of the terminal: Use the optimized global displacement vector to correct the position of each 3D edge feature point in the registered texture point cloud data of each frame to obtain the corrected texture point cloud data of each frame; then generate a high-precision map of the terminal containing color and texture information based on the corrected texture point cloud data of all frames; Real-time self-positioning steps: During the berthing and unberthing process of the ship, the point cloud data currently collected by the lidar is compared with the high-precision map of the terminal in real time, and the feature matching algorithm and positioning algorithm are used to determine the position of the ship in the high-precision map of the terminal in real time, thereby realizing the real-time self-positioning of the ship in the high-precision map of the terminal.

2. The method for berthing and unberthing self-positioning in an intelligent ship based on texture point cloud according to claim 1, characterized in that: In the closed-loop detection and graph model optimization steps, g2o is first used to construct a graph model representing the geometric constraints and feature matching relationships between point cloud data, with the texture point cloud data after each frame of registration as nodes and the displacement vectors between each pair of adjacent frames in the preliminary position trajectory as edges; a closed-loop detection is initiated every 20 frames of laser point cloud collected, and the 3D edge feature points in the registered texture point cloud data are analyzed to determine whether a closed loop exists; when the number of closed-loop constraints in the graph model is greater than or equal to a preset threshold, the g2o graph optimization algorithm and the NDT-color algorithm are comprehensively used to optimize the graph model, and by adjusting the constraint relationship between nodes in the graph model, the accumulated error is detected and corrected, and the optimized mileage estimate is regenerated, and an optimized global displacement vector is generated based on the optimized mileage estimate.

3. The method for berthing and unberthing self-positioning in an intelligent ship based on texture point cloud according to claim 1 or 2, characterized in that: In the step of constructing the high-precision map of the terminal, after position correction is performed on each 3D edge feature point, a filtering algorithm is used to remove noise points and outliers from each corrected 3D edge feature point; the filtering algorithm includes statistical filtering and voxel grid filtering.

4. The method for berthing and unberthing self-positioning in an intelligent ship based on texture point cloud according to claim 1 or 2, characterized in that: In the extrinsic parameter matrix calculation step, the feature extraction algorithm includes the SURF algorithm and the SIFT algorithm; the feature point matching method is a feature point matching method based on the Superglue neural network; the extrinsic parameter matrix of the laser radar relative to the camera includes a rotation matrix and a translation vector; the nonlinear optimization algorithm includes the EPnP algorithm and the iterative closest point algorithm.

5. The method for berthing and unberthing self-positioning in an intelligent ship based on texture point cloud according to claim 1 or 2, characterized in that: In the data registration and open-loop mileage estimation steps, the geometric features include 3D edge feature point coordinates, normal vectors, and curvatures.

6. A texture point cloud-based berthing and unberthing self-positioning system for intelligent ships, characterized by: It includes a chessboard image acquisition module, an internal parameter calibration module, an environmental data synchronization acquisition and orthogonal processing module, an external parameter matrix calculation module, a point cloud texture mapping fusion module, a data registration and open-loop mileage estimation module, a closed-loop detection and graph model optimization module, a terminal high-precision map construction module, and a real-time self-positioning module. The checkerboard image acquisition module uses a fisheye camera to shoot a checkerboard calibration plate pre-set at a fixed position on the dock from different angles to obtain multiple checkerboard images; The intrinsic parameter calibration module uses a computer vision algorithm to detect the 2D pixel coordinates of all inner corner points of the checkerboard in each checkerboard image, and establishes 3D world coordinates corresponding to the 2D pixel coordinates of each inner corner point based on the physical size of the checkerboard in each checkerboard image. Based on the 2D pixel coordinates and 3D world coordinates of each inner corner point, the intrinsic parameter matrix and distortion coefficient of the fisheye camera are calculated using the Zhang calibration method to complete the intrinsic parameter calibration of the fisheye camera. The environmental data synchronous acquisition and orthogonal processing module uses a laser radar and a calibrated fisheye camera to synchronously acquire environmental data around the dock in real time, obtaining multiple point cloud data collected by the laser radar and multiple environmental images collected by the calibrated fisheye camera; and uses the distortion coefficient to perform orthogonal processing on the environmental image to obtain an orthogonalized environmental image; The extrinsic parameter matrix calculation module extracts the coordinates of all 3D edge feature points in the point cloud data in the laser radar coordinate system and the coordinates of all 2D feature points in the orthogonalized environmental image in the camera coordinate system based on the feature extraction algorithm, obtains a point cloud feature set and an environmental image feature set, and uses a feature point matching method to match the 3D edge feature points in the point cloud feature set with the 2D feature points in the environmental image feature set to obtain matched feature point pairs; then uses a RANSAC optimization algorithm to optimize the matched feature point pairs to obtain optimized feature point pairs, and calculates the extrinsic parameter matrix of the laser radar relative to the camera based on the optimized feature point pairs and a nonlinear optimization algorithm; The point cloud texture mapping fusion module converts the coordinates of all 3D edge feature points in the point cloud feature set in the laser radar coordinate system into the camera coordinate system through an external parameter matrix, obtains the coordinates of all 3D edge feature points in the camera coordinate system, and projects the coordinates of all 3D edge feature points in the camera coordinate system onto the orthogonalized environment image through an internal parameter matrix to obtain the corresponding projected pixel coordinates; then uses bilinear interpolation to extract the RGB color value of each projected pixel coordinate from the orthogonalized environment image, and assigns the RGB color value to the corresponding 3D edge feature point according to the spatial mapping rule, thereby generating texture point cloud data with color information and texture information; The data registration and open-loop mileage estimation module introduces geometric features and RGB color values in texture point cloud data on the basis of the NDT algorithm to construct an improved NDT-color algorithm, and uses the improved NDT-color algorithm to align and register each 3D edge feature point in two adjacent frames of texture point cloud data to obtain the registered texture point cloud data of each frame; then, based on the coordinates of each 3D edge feature point in the registered texture point cloud data, the center coordinates of each frame of the registered texture point cloud data are calculated; based on the center coordinates of the current frame and the frame before the current frame, the displacement vector of the ship in the current frame relative to the previous frame is calculated, and then the displacement vector between each pair of adjacent frames from the starting frame to the current frame is obtained, and then all displacement vectors from the starting frame to the current frame are accumulated to achieve open-loop mileage estimation and form a preliminary position trajectory; The closed-loop detection and graph model optimization module constructs a graph model using the texture point cloud data after registration of each frame as nodes and the displacement vectors between each pair of adjacent frames in the preliminary position trajectory as edges; Every certain number of frames, the 3D edge feature points in the registered texture point cloud data are detected to see if there is a closed loop. If a closed loop is detected, a closed loop constraint is added to the graph model, and the number of closed loop constraints in the graph model is compared with a preset number threshold. If the closed loop constraints in the graph model are greater than or equal to the preset number threshold, the G2O graph optimization algorithm is used to optimize the graph model to generate an optimized global displacement vector. The terminal high-precision map construction module uses the optimized global displacement vector to correct the position of each 3D edge feature point in the texture point cloud data after registration of each frame, thereby obtaining the corrected texture point cloud data of each frame; and generates a terminal high-precision map containing color and texture information based on the corrected texture point cloud data of all frames; The real-time self-positioning module compares the point cloud data currently collected by the lidar with the high-precision map of the terminal in real time during the ship's berthing and unberthing process, and uses feature matching algorithm and positioning algorithm to determine the ship's position in the high-precision map of the terminal in real time, thereby realizing real-time self-positioning of the ship in the high-precision map of the terminal.

7. The texture point cloud-based berthing and unberthing self-positioning system for intelligent ships according to claim 6, characterized in that: In the closed-loop detection and graph model optimization module, G2O is first used to construct a graph model representing the geometric constraints and feature matching relationships between point cloud data, with the texture point cloud data after each frame of registration as nodes and the displacement vectors between each pair of adjacent frames in the preliminary position trajectory as edges. After every 20 frames of laser point cloud are collected, a closed-loop detection is initiated to analyze the 3D edge feature points in the registered texture point cloud data to determine whether a closed loop exists. When the number of closed-loop constraints in the graph model is greater than or equal to a preset threshold, the graph model is optimized using a comprehensive G2O graph optimization algorithm and an NDT-color algorithm. By adjusting the constraint relationship between nodes in the graph model, the accumulated error is detected and corrected, and the optimized mileage estimate is regenerated. Based on the optimized mileage estimate, an optimized global displacement vector is generated.

8. The texture point cloud-based berthing and unberthing self-positioning system for intelligent ships according to claim 6 or 7, characterized in that: In the terminal high-precision map construction module, after position correction is performed on each 3D edge feature point, a filtering algorithm is used to remove noise points and outliers from each corrected 3D edge feature point; the filtering algorithm includes statistical filtering and voxel grid filtering.

9. The texture point cloud-based berthing and unberthing self-positioning system for intelligent ships according to claim 6 or 7, characterized in that: In the external parameter matrix calculation module, the feature extraction algorithm includes the SURF algorithm and the SIFT algorithm; the feature point matching method is a feature point matching method based on the Superglue neural network; the external parameter matrix of the laser radar relative to the camera includes a rotation matrix and a translation vector; the nonlinear optimization algorithm includes the EPnP algorithm and the iterative closest point algorithm.

10. The texture point cloud-based berthing and unberthing self-positioning system for intelligent ships according to claim 6 or 7, characterized in that: In the data registration and open-loop mileage estimation module, the geometric features include 3D edge feature point coordinates, normal vectors, and curvatures.

Images