Autonomous berthing and unberthing method and system for unmanned ships based on vision laser fusion
Through visual laser fusion technology, the problems of low accuracy of the automatic off-mooring median posture estimation and low path planning efficiency are solved, high-precision posture estimation and path planning are realized, and the intelligence of the automatic off-mooring system is improved. stability.
Patent Information
- Application Number
- CN202411169990.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-26
- Publication Date
- 2024-11-19
- Estimated Expiration
- 2044-08-26
AI Technical Summary
In the automatic ship off-mooring technology, the hull position estimation accuracy and the path planning efficiency are low, resulting in the automatic off-mooring system having safety hazards and low efficiency in practical applications.
Using a method based on visual laser fusion, a hull posture and position estimation model is established by acquiring dock area images and three-dimensional point cloud data, and combining the visual laser fusion model to generate a composite potential field function, optimize the motion trajectory, and realize high-precision pose estimation and Path planning.
It improves the accuracy of hull position estimation and the efficiency of path planning, realizes high intelligence and full-process closed-loop automatic mooring, and improves the performance and stability of the system.
Smart Images

Figure CN118691999B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to image processing technology, and in particular to an autonomous berthing and unberthing method and system for an unmanned ship based on visual laser fusion. Background Art
[0002] Automatic berthing and unberthing of ships is one of the important links in the automation of terminal operations, which is of great significance for improving port operation efficiency and reducing the risks of manual operation. Traditional berthing and unberthing operations of ships mainly rely on the experience and judgment of the crew, and there are certain safety hazards and low efficiency problems. Therefore, the development of an automated ship berthing and unberthing system has important theoretical and practical significance. At present, the automatic berthing and unberthing technology of ships mainly faces the following key technical difficulties: First, the accurate position (position and attitude) information of the ship is the prerequisite for automatic berthing and unberthing. Due to the complex dock environment and the disturbance of the hull motion, how to obtain high-precision hull position estimation from multi-source heterogeneous sensor data is a major challenge. Second, for different dock layouts, obstacle distributions and ship dynamics constraints, it is necessary to design an efficient and safe berthing and unberthing path planning algorithm, fully considering multiple constraints such as obstacle avoidance and trajectory smoothness. In response to the above problems, domestic and foreign scholars and enterprises have proposed a variety of theoretical and technical solutions through years of research and practical exploration, and have made some progress. However, due to the high technical difficulty, the current automatic berthing and unberthing system of ships still has certain limitations in practical applications, and further technological breakthroughs are urgently needed. Summary of the invention
[0003] The embodiments of the present invention provide a method and system for autonomous berthing and unberthing of an unmanned ship based on vision laser fusion, which can solve the problems in the prior art.
[0004] According to a first aspect of the embodiments of the present invention,
[0005] Provided is an autonomous berthing and unberthing method for an unmanned ship based on visual laser fusion, including:
[0006] Acquire and preprocess the dock area image to obtain the dock edge features, establish a hull attitude estimation model based on the relative position relationship between the dock edge features and the hull contour, solve the initial attitude angle of the hull relative to the dock, and optimize the initial attitude angle to obtain the optimal hull attitude estimation result;
[0007] The three-dimensional point cloud data of the dock area is obtained by laser radar and preprocessed to obtain the dock plane image, extract the dock plane point cloud and construct the dock plane equation, calculate the initial displacement of the hull relative to the dock based on the dock plane equation, and construct the laser measurement error compensation equation to optimize the initial displacement to obtain the optimal hull position estimation result;
[0008] The obtained optimal hull attitude estimation result and the optimal hull position estimation result are input into the pre-trained visual laser fusion model to obtain the optimal hull posture fusion estimation result;
[0009] According to the fusion estimation results of the optimal hull posture and the preset target dock position, a composite potential field function is constructed based on the repulsive force between the ship and the dock and obstacles and the gravitational force of the target dock position. The expected movement direction of the ship at each position is obtained by solving the gradient of the composite potential field function, and the berthing and unberthing path curve is generated.
[0010] The generated berthing and unberthing path curve is divided into multiple segments, and each segment is fitted using a quintic polynomial function. The tangent direction and curvature of each discrete point on the berthing and unberthing path curve are calculated using the derivative information of the polynomial function. The discrete points are screened and adjusted according to the speed and acceleration constraints of the ship to obtain the initial motion trajectory.
[0011] The obtained initial motion trajectory is used as the initial solution, and the motion trajectory optimization problem is constructed in combination with the dynamic model of the ship. The dynamic model of the ship can include propeller thrust and rudder angle limitations. At the same time, smoothing factors and safety constraints are introduced, and the initial motion trajectory is optimized using a nonlinear optimization algorithm to finally obtain an executable motion trajectory.
[0012] In an optional embodiment,
[0013] The initial attitude angle is optimized to obtain the optimal hull attitude estimation results including:
[0014] Obtain the edge features of the dock and the contour features of the hull to construct a hull posture graph, wherein each node in the posture graph represents the posture state of the hull in a frame of image, the posture state includes the position and direction of the hull, and each edge represents the spatial constraint relationship between different posture states;
[0015] According to the correspondence between the edge features of the wharf in different frames, a reprojection error equation is established to project the three-dimensional points in one frame of image to another frame of image and calculate the reconstruction error of the edge features of the wharf;
[0016] According to the transformation relationship of the hull contour features in different frame images, a geometric error equation is established to calculate the geometric error of the hull contour features;
[0017] The reprojection error of the dock edge feature and the geometric error of the hull contour feature are taken as optimization targets, a robust kernel function is introduced, and a graph optimization problem is constructed. By solving the graph optimization problem, the first hull posture estimation result is obtained.
[0018] Based on the first hull attitude estimation result, the dock area image is downsampled to generate image pyramids of different resolutions. The corresponding attitude graph is constructed at each resolution. A multi-scale optimization strategy is introduced to optimize the attitude graph. The attitude estimation result obtained by multi-scale optimization is used as the initial value. The attitude graph is further optimized at the original resolution to obtain the optimal hull attitude estimation result.
[0019] In an optional embodiment,
[0020] Solving the graph optimization problem involves:
[0021] The objective function is constructed by combining the reprojection error of the dock edge features and the geometric error of the hull profile features. The sum of the robust kernel function values of all reprojection errors and geometric errors is minimized through the nonlinear least squares optimization algorithm to solve the graph optimization problem. The objective function formula is as follows:
[0022] ;
[0023] Where E(x) represents the objective function, ρ represents the robust kernel function, and e ij represents the reprojection error or geometric error of the jth feature point in the i-th frame image, λ 1 represents the weight coefficient of the regularization term of the pose node position, X i represents the position of the i-th posture node, X 0i represents the initial position of the i-th posture node, λ 2 Represents the weight coefficient of the regularization term of the attitude node direction, R i represents the direction of the i-th posture node, R 0i Represents the initial direction of the i-th posture node.
[0024] In an optional embodiment,
[0025] Extracting the dock plane point cloud and constructing the dock plane equation include:
[0026] Extract several groups of points from the pre-extracted dock plane point cloud, calculate the candidate plane equation, calculate the distances from all points to the candidate plane according to the candidate plane equation, determine the inner point set supporting the candidate plane according to the pre-set distance threshold, and perform repeated iterative updates to obtain the inner point set of the optimal candidate plane;
[0027] Calculate the weight of each point in the inner point set of the optimal candidate plane, introduce the weight decay parameter, and use the weighted least squares method to perform plane fitting on the inner point set of the optimal candidate plane to obtain the final dock plane equation;
[0028] The calculation formula for the weight of each point in the inner point set of the optimal candidate plane is as follows:
[0029] ;
[0030] Among them, ω t represents the weight of the t-th point, A represents the x-axis parameter of the plane equation, B represents the y-axis parameter of the plane equation, C represents the constant parameter of the plane equation, x t represents the x-axis coordinate of the t-th point, y t represents the y-axis coordinate of the t-th point, z t represents the z-axis coordinate of the t-th point, and σ represents the parameter that controls the weight decay rate.
[0031] In an optional embodiment,
[0032] Training the Vision Laser Fusion model includes:
[0033] The visual laser fusion model is regarded as a generator, and a discriminator network is introduced to discriminate the generated optimal hull posture estimation results and the optimal hull position estimation results. The input of the discriminator network is the hull posture and hull position, and the output layer uses an activation function to map the output to a probability value indicating that the input hull posture is a real sample.
[0034] Input the actual visual laser data pair into the fusion model to generate the corresponding hull pose estimation, where the hull pose estimation includes hull attitude estimation and hull position estimation;
[0035] The generated hull pose estimation and the actual hull pose are input into the discriminator network to calculate the discrimination loss, and the parameters of the discriminator network are updated using the discrimination loss. At the same time, the generated hull pose estimation is input into the updated discriminator to calculate the discrimination error rate.
[0036] The fusion loss function of the visual laser fusion model is constructed according to the hull pose estimation loss and the discrimination error rate of the discriminator. The parameters of the laser vision fusion model are updated by minimizing the loss function, and the update is repeated until the preset number of training rounds is reached to obtain a trained visual laser fusion model.
[0037] In an optional embodiment,
[0038] The calculation formula of the composite potential field function is as follows:
[0039] ;
[0040] Where U(p) represents the composite potential field function, k att represents the gain coefficient of gravity, p represents the ship position, and p g represents the target dock location, k represents the kth obstacle, k rep represents the gain coefficient of repulsive force, p krepresents the obstacle position, D(·) represents the Euclidean distance, and D 0 Indicates the range of influence of repulsive force.
[0041] According to a second aspect of the embodiments of the present invention,
[0042] Provided is an unmanned ship autonomous berthing and unberthing system based on visual laser fusion, including:
[0043] The first unit is used to obtain and pre-process the dock area image to obtain the dock edge features, establish a hull attitude estimation model based on the relative position relationship between the dock edge features and the hull contour, solve the initial attitude angle of the hull relative to the dock, and optimize the initial attitude angle to obtain the optimal hull attitude estimation result;
[0044] The second unit is used to obtain the three-dimensional point cloud data of the dock area through the laser radar and perform preprocessing to obtain the dock plane image, extract the dock plane point cloud and construct the dock plane equation, calculate the initial displacement of the hull relative to the dock based on the dock plane equation, and construct the laser measurement error compensation equation to optimize the initial displacement to obtain the optimal hull position estimation result;
[0045] The third unit is used to input the obtained optimal hull posture estimation result and the optimal hull position estimation result into a pre-trained visual laser fusion model to obtain the optimal hull posture fusion estimation result, and according to the hull posture fusion estimation result and the preset target dock position, use the pre-built ship berthing and unberthing path planning model to obtain the optimal berthing and unberthing path curve, and convert the path curve into an executable motion trajectory.
[0046] According to a third aspect of the embodiments of the present invention,
[0047] An electronic device is provided, comprising:
[0048] processor;
[0049] a memory for storing processor-executable instructions;
[0050] The processor is configured to call the instructions stored in the memory to execute the aforementioned method.
[0051] According to a fourth aspect of the embodiments of the present invention,
[0052] A computer-readable storage medium is provided, on which computer program instructions are stored. When the computer program instructions are executed by a processor, the aforementioned method is implemented.
[0053] In this embodiment, by fusing visual and laser sensor data, and using an optimization algorithm to correct the initial attitude angle and displacement of the hull, a high-precision hull posture (position and attitude) estimation result can be obtained. This provides a reliable posture reference for subsequent automatic berthing and departure. A method of extracting the edge features of the dock from the dock area image using image processing technology, and modeling its relative position relationship with the hull contour, realizes the estimation of the initial attitude angle of the hull, and improves the intelligence level of the system. By constructing a laser measurement error compensation equation based on the dock plane equation, the initial displacement is optimized, the accuracy of laser ranging is improved, and a more accurate hull position estimation result is obtained. The high-precision hull posture fusion estimation result and the target dock position are input into a pre-built path planning model, and the optimal berthing and departure path curve can be obtained, which improves the efficiency and quality of path planning. The optimal path curve obtained by planning is converted into an executable motion trajectory, realizing the full process closed loop of automatic berthing and departure control, improving the automation level of dock operations, realizing the high intelligence of the system, and optimizing algorithms in various links, improving the performance and stability of the overall system. BRIEF DESCRIPTION OF THE DRAWINGS
[0054] Figure 1 It is a flow chart of an autonomous berthing and unberthing method for an unmanned ship based on vision laser fusion according to an embodiment of the present invention;
[0055] Figure 2 It is a schematic diagram of the structure of an unmanned ship autonomous berthing and unberthing system based on vision laser fusion according to an embodiment of the present invention. DETAILED DESCRIPTION
[0056] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0057] The technical solution of the present invention is described in detail with specific embodiments below. The following specific embodiments can be combined with each other, and the same or similar concepts or processes may not be described in detail in some embodiments.
[0058] Figure 1 FIG. 1 is a flow chart of an autonomous berthing and unberthing method for an unmanned ship based on visual laser fusion according to an embodiment of the present invention. Figure 1 As shown, the method includes:
[0059] S101. Acquire and preprocess the dock area image to obtain the dock edge features, establish a hull attitude estimation model using the relative position relationship between the dock edge features and the hull contour, solve the initial attitude angle of the hull relative to the dock, and optimize the initial attitude angle to obtain the optimal hull attitude estimation result.
[0060] Among them, the Canny operator can be used to detect the edge of the image, and then the straight edge of the dock can be extracted through Hough transform. In order to improve the robustness, the length and slope of the line segment can be added on this basis to filter out the real edge line of the dock.
[0061] According to the relative position relationship between the dock edge line and the hull contour, the roll angle, pitch angle and heading angle of the hull relative to the dock are estimated by analytical method or optimization method. Among them, the roll angle and pitch angle can be directly obtained from the angle between the hull contour and the horizontal line, and the heading angle needs to be estimated based on the symmetry of the hull contour.
[0062] Finally, a visual measurement error model is established through calibration experiments, and corresponding compensation is performed in the posture estimation results.
[0063] In an optional embodiment,
[0064] The initial attitude angle is optimized to obtain the optimal hull attitude estimation results including:
[0065] Obtain the edge features of the dock and the contour features of the hull to construct a hull posture graph, wherein each node in the posture graph represents the posture state of the hull in a frame of image, the posture state includes the position and direction of the hull, and each edge represents the spatial constraint relationship between different posture states;
[0066] According to the correspondence between the edge features of the wharf in different frames, a reprojection error equation is established to project the three-dimensional points in one frame of image to another frame of image and calculate the reconstruction error of the edge features of the wharf;
[0067] According to the transformation relationship of the hull contour features in different frame images, a geometric error equation is established to calculate the geometric error of the hull contour features;
[0068] The reprojection error of the dock edge feature and the geometric error of the hull contour feature are taken as optimization targets, a robust kernel function is introduced, and a graph optimization problem is constructed. By solving the graph optimization problem, the first hull posture estimation result is obtained.
[0069] Based on the first hull attitude estimation result, the dock area image is downsampled to generate image pyramids of different resolutions. The corresponding attitude graph is constructed at each resolution. A multi-scale optimization strategy is introduced to optimize the attitude graph. The attitude estimation result obtained by multi-scale optimization is used as the initial value. The attitude graph is further optimized at the original resolution to obtain the optimal hull attitude estimation result.
[0070] Exemplarily, first, the hull attitude estimation problem is modeled as a graph optimization problem. A posture graph is defined, in which each node represents a posture state, including the position and orientation of the hull in the frame image. Each edge represents the spatial constraint relationship between two posture states, including reprojection error and geometric error. By minimizing the reprojection error and geometric error, the globally optimal attitude estimation result can be obtained.
[0071] Graph optimization problems can be solved using nonlinear least squares optimization methods such as the Gauss-Newton method or the Levenberg-Marquardt method.
[0072] First, use ICP, PnP and other algorithms to initialize the nodes in the posture graph to obtain the initial posture estimate. According to the definition of reprojection error and geometric error, a least squares optimization problem is constructed. The position and direction of the posture node are used as optimization variables, and the error term is used as the observation value. The least squares problem is solved using the optimization algorithm provided by the g2o library, and the position and direction of the posture node are iteratively updated until the convergence condition is reached or the maximum number of iterations is reached. The optimized position and direction of the posture node are output as the final posture estimation result.
[0073] In order to improve the robustness of graph optimization methods, robust kernel functions can be introduced into the error term. Robust kernel functions can reduce the impact of outliers on the optimization process and improve the stability of the estimation results. Commonly used robust kernel functions include Huber kernel function, Cauchy kernel function and Tukey kernel function.
[0074] In order to further improve the convergence speed and global optimality of the graph optimization method, a multi-scale optimization strategy can be adopted. The multi-scale optimization strategy optimizes the pose graph at different resolutions by constructing a pyramid representation of the image, and gradually refines the pose estimation result.
[0075] Specifically, the original dock area image is first downsampled to generate image pyramids of different resolutions. At each scale, the corresponding posture graph is constructed based on the dock edge features and hull contour extraction results. Starting from the coarsest scale, the posture graph is optimized. The optimization result is used as the initial value of the next scale, and the optimization is iterated in sequence until the finest scale is reached. At the finest scale, the posture graph is further optimized to obtain the final posture estimation result.
[0076] In an optional embodiment,
[0077] Solving the graph optimization problem involves:
[0078] The objective function is constructed by combining the reprojection error of the dock edge features and the geometric error of the hull profile features. The sum of the robust kernel function values of all reprojection errors and geometric errors is minimized through the nonlinear least squares optimization algorithm to solve the graph optimization problem. The objective function formula is as follows:
[0079] ;
[0080] Where E(x) represents the objective function, ρ represents the robust kernel function, and e ij represents the reprojection error or geometric error of the jth feature point in the i-th frame image, λ 1 represents the weight coefficient of the regularization term of the pose node position, X i represents the position of the i-th posture node, X 0i represents the initial position of the i-th posture node, λ 2 Represents the weight coefficient of the regularization term of the attitude node direction, R i represents the direction of the i-th posture node, R 0i Represents the initial direction of the i-th posture node.
[0081] In this embodiment, by constructing a hull attitude graph and fusing the dock edge features and the hull contour features, the attitude estimation problem is modeled as a graph optimization problem and solved, and a high-precision hull attitude estimation result can be obtained. The introduction of a robust kernel function can effectively suppress the influence of outliers and improve the robustness of the algorithm to image noise and occlusion. The multi-scale optimization strategy of the image pyramid is adopted to optimize the attitude graph at different resolutions, and the attitude estimation is gradually optimized in a coarse-to-fine manner, which improves the robustness and convergence speed of the algorithm. At the same time, the dock edge features and the hull contour features are used to fuse two different types of feature information, thereby improving the accuracy and reliability of attitude estimation. By constructing a attitude graph, the attitude state of the hull in different frame images and its spatial constraint relationship are modeled, which reflects the rationality and integrity of the algorithm. By establishing a reprojection error equation and a geometric error equation, the reprojection error of the dock edge features and the geometric error of the hull contour features are simultaneously optimized during the optimization process, thereby improving the accuracy of visual measurement.
[0082] S102. Acquire the three-dimensional point cloud data of the dock area through the laser radar and perform preprocessing to obtain the dock plane image, extract the dock plane point cloud and construct the dock plane equation, calculate the initial displacement of the hull relative to the dock based on the dock plane equation, and construct the laser measurement error compensation equation to optimize the initial displacement to obtain the optimal hull position estimation result.
[0083] For example, the three-dimensional point cloud data of the dock area is first obtained by laser radar scanning, and then the dock plane is fitted using the RANSAC algorithm. In order to improve the computational efficiency, the point cloud can be pre-processed by downsampling and voxel filtering.
[0084] Using the reconstructed dock plane as a reference, the current frame laser point cloud is matched with the reference frame point cloud through the ICP algorithm to estimate the 3D displacement of the hull relative to the dock. To avoid mismatching, features such as the dock edge and shoreline can be added as constraints.
[0085] According to the characteristics of the dock environment, such as dock material, temperature, humidity, etc., a laser measurement error compensation model is established. The error compensation parameters are estimated using the least squares method or Kalman filter to obtain the compensation value, which is applied to the hull position estimation result to obtain the final hull position.
[0086] In an optional embodiment,
[0087] Extracting the dock plane point cloud and constructing the dock plane equation include:
[0088] Extract several groups of points from the pre-extracted dock plane point cloud, calculate the candidate plane equation, calculate the distances from all points to the candidate plane according to the candidate plane equation, determine the inner point set supporting the candidate plane according to the pre-set distance threshold, and perform repeated iterative updates to obtain the inner point set of the optimal candidate plane;
[0089] Calculate the weight of each point in the inner point set of the optimal candidate plane, introduce the weight decay parameter, and use the weighted least squares method to perform plane fitting on the inner point set of the optimal candidate plane to obtain the final dock plane equation;
[0090] The calculation formula for the weight of each point in the inner point set of the optimal candidate plane is as follows:
[0091] ;
[0092] Among them, ω t represents the weight of the t-th point, A represents the x-axis parameter of the plane equation, B represents the y-axis parameter of the plane equation, C represents the constant parameter of the plane equation, x t represents the x-axis coordinate of the t-th point, y t represents the y-axis coordinate of the t-th point, z t represents the z-axis coordinate of the t-th point, and σ represents the parameter that controls the weight decay rate.
[0093] The number of iterations can be estimated based on the proportion of inliers and the desired confidence.
[0094] In this embodiment, the inner point set of the optimal candidate plane is determined by repeated iterations, and the distance threshold is introduced, which can effectively eliminate the influence of outliers and improve the robustness of plane fitting. The weight of each inner point is calculated, and the weighted least square method is used for plane fitting, and different points are given different weights, which improves the accuracy of plane fitting. In the iterative process, outlier points are dynamically eliminated by the distance threshold, without manual intervention or initial value setting, and the algorithm is efficient and concise. The obtained dock plane equation can accurately describe the geometric position and posture of the dock plane, and provide a reliable reference plane for the subsequent calculation of the displacement of the hull relative to the dock. By adjusting the distance threshold and weight attenuation parameters, the convergence speed and accuracy of the algorithm can be flexibly controlled, with good adjustability, and the dock plane equation can be accurately extracted from complex three-dimensional point cloud data, which lays the foundation for subsequent displacement calculation and error compensation, improves the accuracy and reliability of laser measurement, and has important theoretical value and application significance for realizing automatic berthing and unberthing of ships.
[0095] S103. Input the obtained optimal hull posture estimation result and the optimal hull position estimation result into the pre-trained visual laser fusion model to obtain the optimal hull posture fusion estimation result, and according to the hull posture fusion estimation result and the preset target dock position, use the pre-built ship berthing and unberthing path planning model to obtain the optimal berthing and unberthing path curve, and convert the path curve into an executable motion trajectory.
[0096] For example, due to the differences in spatial position and time synchronization between the visual and laser perception modules, it is first necessary to determine the internal and external parameters of the two modules through calibration experiments and align the timestamps. The visually estimated hull posture and the laser-estimated hull position are fused to obtain the 6-DOF position of the hull in the dock coordinate system. Multi-sensor fusion can be achieved by using methods such as Kalman filtering and particle filtering.
[0097] According to the navigation plan, the location coordinates of the target dock are determined. Using the pre-built ship berthing and unberthing path planning model, the fusion estimation results of the hull posture and the location of the target dock are used as input to calculate the optimal berthing and unberthing path curve. The converted motion trajectory is sent to the ship's control system to execute the corresponding motion control to achieve automatic berthing and unberthing.
[0098] Optionally, during the automatic berthing and unberthing process, the real-time position information of the ship is continuously obtained and compared with the planned motion trajectory, and the motion trajectory is adjusted and corrected in real time according to the deviation. A safety monitoring mechanism is set up to monitor abnormal conditions during the berthing and unberthing process. At the same time, the interface for manual intervention is retained, and the operator can manually take over control when necessary.
[0099] In an optional embodiment,
[0100] Training the Vision Laser Fusion model includes:
[0101] The visual laser fusion model is regarded as a generator, and a discriminator network is introduced to discriminate the generated optimal hull posture estimation results and the optimal hull position estimation results. The input of the discriminator network is the hull posture and hull position, and the output layer uses an activation function to map the output to a probability value indicating that the input hull posture is a real sample.
[0102] Input the actual visual laser data pair into the fusion model to generate the corresponding hull pose estimation, where the hull pose estimation includes hull attitude estimation and hull position estimation;
[0103] The generated hull pose estimation and the actual hull pose are input into the discriminator network to calculate the discrimination loss, and the parameters of the discriminator network are updated using the discrimination loss. At the same time, the generated hull pose estimation is input into the updated discriminator to calculate the discrimination error rate.
[0104] The fusion loss function of the visual laser fusion model is constructed according to the hull pose estimation loss and the discrimination error rate of the discriminator. The parameters of the laser vision fusion model are updated by minimizing the loss function, and the update is repeated until the preset number of training rounds is reached to obtain a trained visual laser fusion model.
[0105] Exemplarily, a discriminator network is first constructed. The input of the discriminator network is the hull pose, which can be expressed in Euler angles or quaternions. A multi-layer fully connected neural network is used as the main structure of the discriminator, and each layer is followed by a ReLU activation function and BatchNorm normalization. In the output layer of the discriminator, a Sigmoid activation function is used to map the output to a probability value between 0 and 1, indicating the probability that the input hull pose is a real sample. The goal of the discriminator network is to maximize the ability to correctly distinguish between the real hull pose and the generated hull pose.
[0106] The visual laser fusion model is used as a generator. Its goal is not only to minimize the loss function of the hull pose estimation, but also to maximize the discrimination error rate of the discriminator network. The hull pose estimation loss function can adopt common regression loss functions such as Mean Squared Error (MSE) or Mean Absolute Error (MAE). The discrimination error rate of the discriminator network can be expressed as the probability of misjudging the generated hull pose as a real sample. The overall loss function of the fusion model is the weighted sum of the hull pose estimation loss and the discriminator discrimination error rate. The weight coefficient is used to balance the importance of the two loss terms.
[0107] In each training batch, a batch of real visual laser data pairs are randomly sampled and input into the fusion model to generate the corresponding hull pose estimation. The generated hull pose and the real hull pose are respectively input into the discriminator network to calculate the discriminant loss of the discriminator. The parameters of the fusion model are fixed, and the parameters of the discriminator network are updated through back propagation to improve the discriminator's ability to distinguish between the real hull pose and the generated hull pose. The parameters of the discriminator network are fixed, and the hull pose generated by the fusion model is input into the discriminator to calculate the discrimination error rate, and combined with the hull pose estimation loss, the parameters of the fusion model are updated through back propagation. Repeat the above steps until both the fusion model and the discriminator network converge or the preset number of training rounds is reached.
[0108] In the inference phase, only the trained fusion model is used to estimate the hull pose of the new visual laser data. The input data is passed into the fusion model to obtain the generated hull pose estimation result. There is no need to use the discriminator network, and the output of the fusion model is directly used as the final hull pose estimation.
[0109] In this embodiment, the discriminator network is introduced to discriminate the generated hull pose estimation, so that the visual laser fusion model not only needs to fit the training data during the training process, but also needs to deceive the discriminator network, thereby improving the generalization ability of the model and reducing the risk of overfitting. Through adversarial training, the fusion model minimizes the pose estimation loss and the discrimination error rate of the discriminator during the training process, so that the trained model has stronger robustness to abnormal data and noise. During the training process, there is no need to mark the true pose of the hull in advance, but the discriminant loss generated by the discriminator network is used to drive the fusion model to update the parameters, realizing end-to-end unsupervised training. It can fuse visual and laser data at the same time, making full use of the complementarity of different sensors, and obtaining more accurate and reliable hull pose estimation results. The idea of generating adversarial networks is introduced into the training process of the visual laser fusion model, which improves the generalization ability and robustness of the model, realizes unsupervised end-to-end training, can effectively fuse multimodal data, and provides strong technical support for the pose estimation of ships in automatic berthing and leaving.
[0110] In an optional embodiment,
[0111] According to the fusion estimation result of the hull posture and the preset target dock position, the optimal berthing and unberthing path curve is obtained by using the pre-built ship berthing and unberthing path planning model, and the path curve is converted into an executable motion trajectory including:
[0112] According to the fusion estimation results of the hull posture and the preset target dock position, a composite potential field function is constructed based on the repulsive force between the ship and the dock and obstacles and the gravitational force of the target dock position. The expected movement direction of the ship at each position is obtained by solving the gradient of the composite potential field function, and the berthing and unberthing path curve is generated.
[0113] The generated berthing and unberthing path curve is divided into multiple segments, and each segment is fitted using a quintic polynomial function. The tangent direction and curvature of each discrete point on the berthing and unberthing path curve are calculated using the derivative information of the polynomial function. The discrete points are screened and adjusted according to the speed and acceleration constraints of the ship to obtain the initial motion trajectory.
[0114] The obtained initial motion trajectory is used as the initial solution, and the motion trajectory optimization problem is constructed in combination with the dynamic model of the ship. The dynamic model of the ship can include propeller thrust and rudder angle limitations. At the same time, smoothing factors and safety constraints are introduced, and the initial motion trajectory is optimized using a nonlinear optimization algorithm to finally obtain an executable motion trajectory.
[0115] Exemplarily, an improved artificial potential field function is introduced to comprehensively consider the repulsive force between the ship and the dock, obstacles, and the gravitational force of the target dock position to construct a composite potential field function. On the basis of the traditional artificial potential field method, a dynamic attenuation factor is introduced to attenuate the influence of the repulsive force as the distance between the ship and the obstacle increases, thereby avoiding the local minimum problem. Combined with the dynamic characteristics of the ship, such as the bow angle, turning radius, etc., the potential field function is corrected so that the generated path conforms to the kinematic constraints of the ship. By solving the gradient of the composite potential field function, the expected movement direction of the ship at each position is obtained, and a smooth and safe berthing and unberthing path curve is generated.
[0116] The generated berthing and unberthing path curve is divided into multiple segments, and each segment is fitted with a quintic polynomial function. The coefficients of the quintic polynomial function are solved by the least squares method to minimize the error between the fitting curve and the original path curve. The derivative information of the quintic polynomial function is used to calculate the tangent direction and curvature of each discrete point to ensure the smoothness and continuity of the discrete trajectory. According to the speed and acceleration constraints of the ship, the time interval between discrete points is adjusted to generate a time-parameterized motion trajectory.
[0117] The discretized motion trajectory is used as the initial solution, and the motion trajectory optimization problem is constructed in combination with the ship's dynamic model, such as propeller thrust, rudder angle limit, etc. Nonlinear optimization algorithms, such as sequential quadratic programming (SQP) or interior point method, are used to solve the optimization problem and obtain the optimal motion trajectory that satisfies the ship's dynamic constraints. In the optimization process, penalty terms for smoothness and safety are introduced to ensure that the optimized trajectory is still smooth, safe, and dynamically feasible.
[0118] Optionally, an adaptive heading controller is designed to calculate the required rudder angle command according to the deviation between the current position and heading angle of the ship and the desired trajectory. A preview control strategy is adopted to adjust the heading in advance according to the current speed and preview distance of the ship to reduce the trajectory tracking error. An adaptive gain adjustment mechanism is introduced to adjust the controller gain parameters in real time according to factors such as the ship's speed and environmental interference to improve the robustness of trajectory tracking.
[0119] In an optional embodiment,
[0120] The calculation formula of the composite potential field function is as follows:
[0121] ;
[0122] Where U(p) represents the composite potential field function, k att represents the gain coefficient of gravity, p represents the ship position, and p g represents the target dock location, k represents the kth obstacle, k rep represents the gain coefficient of repulsive force, p k represents the obstacle position, D(·) represents the Euclidean distance, and D 0 Indicates the range of influence of repulsive force.
[0123] In this embodiment, by constructing a composite potential field function, the repulsion between the ship and the dock, the obstacle and the gravity of the target dock position are integrated, and the path planning in the global range is realized to avoid falling into the local optimum. The composite potential field function takes into account the location information of the dock and the obstacle, so that the planned path can effectively avoid obstacles and improve the environmental perception ability of the path planning. The path planning and motion trajectory optimization are combined, and the initial trajectory is optimized based on the ship dynamics model, so that the final motion trajectory satisfies the motion constraints of the ship and is executable. In the trajectory optimization process, a smoothing factor is introduced to ensure that the optimized motion trajectory has good smoothness and avoids drastic changes in speed and acceleration. Safety constraints are added to the optimization problem to ensure that the optimized motion trajectory will not collide with obstacles, thereby improving the safety of ship movement. By adjusting the parameters in the composite potential field function, the repulsion strength between the ship and the obstacle and the gravity strength of the target dock position can be flexibly controlled, so that the path planning has good adjustability.
[0124] Figure 2 FIG. 1 is a schematic diagram of the structure of an unmanned ship autonomous berthing and unberthing system based on visual laser fusion according to an embodiment of the present invention. Figure 2 As shown, the system comprises:
[0125] The first unit is used to obtain and pre-process the dock area image to obtain the dock edge features, establish a hull attitude estimation model based on the relative position relationship between the dock edge features and the hull contour, solve the initial attitude angle of the hull relative to the dock, and optimize the initial attitude angle to obtain the optimal hull attitude estimation result;
[0126] The second unit is used to obtain the three-dimensional point cloud data of the dock area through the laser radar and perform preprocessing to obtain the dock plane image, extract the dock plane point cloud and construct the dock plane equation, calculate the initial displacement of the hull relative to the dock based on the dock plane equation, and construct the laser measurement error compensation equation to optimize the initial displacement to obtain the optimal hull position estimation result;
[0127] The third unit is used to input the obtained optimal hull posture estimation result and the optimal hull position estimation result into a pre-trained visual laser fusion model to obtain the optimal hull posture fusion estimation result, and according to the hull posture fusion estimation result and the preset target dock position, use the pre-built ship berthing and unberthing path planning model to obtain the optimal berthing and unberthing path curve, and convert the path curve into an executable motion trajectory.
[0128] According to a third aspect of the embodiments of the present invention,
[0129] An electronic device is provided, comprising:
[0130] processor;
[0131] a memory for storing processor-executable instructions;
[0132] The processor is configured to call the instructions stored in the memory to execute the aforementioned method.
[0133] A fourth aspect of the embodiments of the present invention is:
[0134] A computer-readable storage medium is provided, on which computer program instructions are stored. When the computer program instructions are executed by a processor, the aforementioned method is implemented.
[0135] The present invention may be a method, an apparatus, a system and / or a computer program product. The computer program product may include a computer-readable storage medium carrying computer-readable program instructions for executing various aspects of the present invention.
[0136] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.
Claims
1. An autonomous berthing and unberthing method for an unmanned ship based on visual laser fusion, characterized in that: include: Acquire and preprocess the dock area image to obtain the dock edge features, establish a hull attitude estimation model based on the relative position relationship between the dock edge features and the hull contour, solve the initial attitude angle of the hull relative to the dock, and optimize the initial attitude angle to obtain the optimal hull attitude estimation result; The three-dimensional point cloud data of the dock area is obtained by laser radar and preprocessed to obtain the dock plane image, extract the dock plane point cloud and construct the dock plane equation, calculate the initial displacement of the hull relative to the dock based on the dock plane equation, and construct the laser measurement error compensation equation to optimize the initial displacement to obtain the optimal hull position estimation result; The obtained optimal hull attitude estimation result and the optimal hull position estimation result are input into the pre-trained visual laser fusion model to obtain the optimal hull posture fusion estimation result; According to the fusion estimation results of the optimal hull posture and the preset target dock position, a composite potential field function is constructed based on the repulsive force between the ship and the obstacle and the gravitational force between the ship and the target dock position. The expected movement direction of the ship at each position is obtained by solving the gradient of the composite potential field function, and the berthing and unberthing path curve is generated. The generated berthing and unberthing path curve is divided into multiple segments, and each segment is fitted using a quintic polynomial function. The tangent direction and curvature of each discrete point on the berthing and unberthing path curve are calculated using the derivative information of the polynomial function. The discrete points are screened and adjusted according to the speed and acceleration constraints of the ship to obtain the initial motion trajectory. The obtained initial motion trajectory is used as the initial solution, and the motion trajectory optimization problem is constructed in combination with the ship's dynamic model. The ship's dynamic model includes propeller thrust and rudder angle restrictions. At the same time, smoothing factors and safety constraints are introduced, and the initial motion trajectory is optimized using a nonlinear optimization algorithm, and finally an executable motion trajectory is obtained; The initial attitude angle is optimized to obtain the optimal hull attitude estimation results including: Obtain the edge features of the dock and the contour features of the hull to construct a hull posture graph, wherein each node in the posture graph represents the posture state of the hull in a frame of image, the posture state includes the position and direction of the hull, and each edge represents the spatial constraint relationship between different posture states; According to the correspondence between the edge features of the wharf in different frames, a reprojection error equation is established, and the three-dimensional points in one frame of image are projected into another frame of image to calculate the reprojection error of the edge features of the wharf; According to the transformation relationship of the hull contour features in different frame images, a geometric error equation is established to calculate the geometric error of the hull contour features; The reprojection error of the dock edge feature and the geometric error of the hull contour feature are taken as optimization targets, a robust kernel function is introduced, and a graph optimization problem is constructed. By solving the graph optimization problem, the first hull posture estimation result is obtained. Based on the first hull attitude estimation result, the dock area image is downsampled to generate image pyramids of different resolutions, and the corresponding attitude graph is constructed at each resolution. The multi-scale optimization strategy is introduced to optimize the attitude graph. The attitude estimation result obtained by multi-scale optimization is used as the initial value, and the attitude graph is further optimized at the original resolution to obtain the optimal hull attitude estimation result. Training the Vision Laser Fusion model includes: The visual laser fusion model is regarded as a generator, and a discriminator network is introduced to discriminate the generated optimal hull posture estimation results and the optimal hull position estimation results. The input of the discriminator network is the hull posture and hull position, and the output layer uses an activation function to map the output to a probability value indicating that the input hull posture is a real sample. Input the actual visual laser data pair into the fusion model to generate the corresponding hull pose estimation, where the hull pose estimation includes hull attitude estimation and hull position estimation; The generated hull pose estimation and the actual hull pose are input into the discriminator network to calculate the discrimination loss, and the parameters of the discriminator network are updated using the discrimination loss. At the same time, the generated hull pose estimation is input into the updated discriminator to calculate the discrimination error rate. The fusion loss function of the visual laser fusion model is constructed according to the hull pose estimation loss and the discrimination error rate of the discriminator. The parameters of the laser vision fusion model are updated by minimizing the loss function, and the update is repeated until the preset number of training rounds is reached to obtain a trained visual laser fusion model.
2. The method according to claim 1, characterized in that Solving the graph optimization problem involves: The objective function is constructed based on the reprojection error of the dock edge feature and the geometric error of the hull profile feature. The sum of the robust kernel function values of all reprojection errors and geometric errors is minimized through the nonlinear least squares optimization algorithm to solve the graph optimization problem. The objective function formula is as follows: ; Where E(x) represents the objective function, ρ represents the robust kernel function, and e ij represents the reprojection error or geometric error of the jth feature point in the i-th frame image, λ1 represents the weight coefficient of the regularization term of the pose node position, X i represents the position of the i-th posture node, X 0i represents the initial position of the i-th posture node, λ2 represents the weight coefficient of the posture node direction regularization term, R i represents the direction of the i-th posture node, R 0i represents the initial direction of the i-th posture node.
3. The method according to claim 1, characterized in that: Extracting the dock plane point cloud and constructing the dock plane equation include: Extract several groups of points from the pre-extracted dock plane point cloud, calculate the candidate plane equation, calculate the distances from all points to the candidate plane according to the candidate plane equation, determine the inner point set supporting the candidate plane according to the pre-set distance threshold, and perform repeated iterative updates to obtain the inner point set of the optimal candidate plane; Calculate the weight of each point in the inner point set of the optimal candidate plane, introduce the weight decay parameter, and use the weighted least squares method to perform plane fitting on the inner point set of the optimal candidate plane to obtain the final dock plane equation; The calculation formula for the weight of each point in the inner point set of the optimal candidate plane is as follows: ; Among them, ω t represents the weight of the t-th point, A represents the x-axis parameter of the plane equation, B represents the y-axis parameter of the plane equation, C represents the constant parameter of the plane equation, x t represents the x-axis coordinate of the t-th point, y t represents the y-axis coordinate of the t-th point, z t represents the z-axis coordinate of the t-th point, and σ represents the parameter that controls the weight decay rate.
4. The method according to claim 1, characterized in that: The calculation formula of the composite potential field function is as follows: ; Where U(p) represents the composite potential field function, k att represents the gain coefficient of gravity, p represents the ship position, and p g represents the target dock location, k represents the kth obstacle, k rep represents the gain coefficient of repulsive force, p k represents the obstacle position, D(·) represents the Euclidean distance, and D0 represents the influence range of the repulsive force.
5. An unmanned ship autonomous berthing and unberthing system based on visual laser fusion, used to implement the method described in any one of claims 1 to 4, characterized in that: include: The first unit is used to obtain and pre-process the dock area image to obtain the dock edge features, establish a hull attitude estimation model based on the relative position relationship between the dock edge features and the hull contour, solve the initial attitude angle of the hull relative to the dock, and optimize the initial attitude angle to obtain the optimal hull attitude estimation result; The second unit is used to obtain the three-dimensional point cloud data of the dock area through the laser radar and perform preprocessing to obtain the dock plane image, extract the dock plane point cloud and construct the dock plane equation, calculate the initial displacement of the hull relative to the dock based on the dock plane equation, and construct the laser measurement error compensation equation to optimize the initial displacement to obtain the optimal hull position estimation result; The third unit is used to input the obtained optimal hull posture estimation result and the optimal hull position estimation result into a pre-trained visual laser fusion model to obtain the optimal hull posture fusion estimation result, and according to the hull posture fusion estimation result and the preset target dock position, use the pre-built ship berthing and unberthing path planning model to obtain the optimal berthing and unberthing path curve, and convert the path curve into an executable motion trajectory.
6. An electronic device, characterized in that: include: processor; a memory for storing processor-executable instructions; The processor is configured to call the instructions stored in the memory to execute the method described in any one of claims 1 to 4.
7. A computer-readable storage medium having computer program instructions stored thereon, characterized in that: When the computer program instructions are executed by a processor, the method according to any one of claims 1 to 4 is implemented.
