Unmanned ship positioning method and berth selection method based on high-precision semantic map

Abstract

The invention discloses an unmanned ship positioning method and a berth selection method based on a high-precision semantic map, and the method comprises the steps: carrying out the semantic segmentation of a three-dimensional point cloud of a berth area scene, and extracting angular points and plane points based on the curvature; constructing a local sub-graph according to the historical key frame point cloud, and constructing a semantic weighting-based line-plane matching residual error between the current frame point cloud and the local sub-graph based on the angular points and plane points of the current frame point cloud; constructing a shore surface matching residual error based on fitting prediction between the fitting shore surface of the current frame and the fitting shore surface of the subgraph; and forming a semantic laser odometer factor by the semantic weighting-based line-surface matching residual error and the fitting prediction-based shore surface matching residual error, further constructing an overall factor graph, and optimizing to obtain the optimal global pose of the unmanned ship. According to the method, the structured features are subjected to weighted matching through semantic information and constraint constructed by utilizing a fitting prediction shore surface are jointly added into speedometer optimization, so that real-time high-precision positioning of the unmanned ship can be realized.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned ships, and in particular to an unmanned ship positioning method and a berth selection method based on a high-precision semantic map. Background Art

[0002] As unmanned ships gradually become more familiar to the public, their research and application are becoming more and more extensive. In the military, they are used to perform high-risk tasks such as minesweeping, reconnaissance, port security and anti-piracy; in the civil field, they are often used for regular inspections, water quality testing, ocean mapping, maritime monitoring, disaster emergency response and search and rescue. Under the demand for intelligent development, unmanned ships must have autonomous operation capabilities.

[0003] In the autonomous operation of unmanned ships on water, accurate environmental perception and autonomous navigation are crucial, and the accurate construction of maps provides the prerequisite for it. It can not only reflect the physical characteristics of the environment in which the unmanned ship is located in detail, such as the shore contour, obstacle location, waterway depth, etc., but also help the unmanned ship plan the optimal path, avoid collision accidents, and improve the safety and efficiency of operations. Based on the SLAM (Simultaneous Localization and Mapping) technology, the introduction of semantic segmentation and semantic information further enhances the environmental perception ability of unmanned ships. By classifying and labeling different objects in the environment, including docks, buoys, ships, etc., these semantic information enables unmanned ships to not only see objects in the environment, but also understand the nature and function of these objects. For example, identifying dock areas and water channels can help unmanned ships better plan berthing paths and avoid entering dangerous areas.

[0004] At present, SLAM technology has been widely used in unmanned driving, drones, etc., but relatively less in unmanned ships. Traditional unmanned ships mostly use GPS for positioning and navigation. By summarizing and analyzing the current research status of unmanned ship perception positioning and SLAM, the shortcomings are as follows:

[0005] 1. Most maps used on the water are electronic river maps, which have low accuracy and poor environmental representation capabilities. They are unable to obtain detailed three-dimensional spatial information about the environment, and unmanned boats cannot obtain information about surrounding dynamic ships and make timely decisions on collision avoidance and planning, which cannot meet the needs of autonomous berthing.

[0006] 2. Most of the current SLAM algorithms are applied to unmanned vehicles or robots. In the water environment of autonomous berthing, there are relatively fewer feature points and the accuracy is reduced, which makes it difficult to directly apply to unmanned ships. The shore or shoreline is an important reference information in the berthing process, which is not considered by the current methods. Secondly, most SLAM algorithms only use geometric features and do not consider the semantic information of different objects. There are many mismatches, which affects the accuracy.

[0007] 3. Currently, when ships berth, they are limited by the complexity of dynamic scenes, the loss of positioning signals, and the real-time and variable behavior decisions, making it difficult to make reasonable berth selections in a timely manner. Summary of the invention

[0008] In view of the above defects of the prior art, the present invention provides an unmanned ship positioning method and a berth selection method based on a high-precision semantic map, and realizes unmanned ship positioning and berth selection based on the constructed high-precision semantic map.

[0009] To achieve the above object, according to a first aspect of the present invention, a method for positioning an unmanned ship based on a high-precision semantic map is provided, the method comprising:

[0010] Perform semantic segmentation on the 3D point cloud of the parking area scene and extract corner points and plane points based on curvature;

[0011] Construct a local sub-graph based on the historical key-frame point cloud, and construct a semantically weighted line-surface matching residual between the current frame point cloud and the local sub-graph based on the corner points and plane points of the current frame point cloud;

[0012] Predict the fitted shore surface of the current frame based on the fitted shore surface of the previous frame and the incremental pose between the point cloud of the current frame and the point cloud of the previous frame;

[0013] Extracting a sub-image fitting shore surface based on the local sub-image, and constructing a shore surface matching residual based on fitting prediction between the fitting shore surface of the current frame and the sub-image fitting shore surface;

[0014] The semantically weighted line-surface matching residual and the fitting-prediction-based shore-surface matching residual constitute the semantic laser odometry factor. The overall factor graph is constructed by fusing the semantic laser odometry factor with at least one of the IMU pre-integration factor, the closed-loop detection factor and the GPS factor to optimize the optimal global pose of the unmanned ship.

[0015] In the above scheme, semantic segmentation of the 3D point cloud of the parking area scene is performed, including:

[0016] The fully convolutional semantic segmentation network is used to perform semantic segmentation on the 3D point cloud of the parking area scene, with the projected image as input and the depth image as output to obtain the semantic segmentation result;

[0017] Semantic segmentation of the 3D point cloud of the parking area scene also includes:

[0018] Each pixel of the depth image is post-processed based on neighborhood search to further determine the semantic category of each pixel, which is used as the final semantic segmentation result, as follows:

[0019] For each pixel in the depth image, take it as the center point and build a window of size l×l; use the current pixel coordinates as the center point to build a Gaussian probability density function, and bring each pixel in the window into the probability density of the corresponding pixel, as shown in the following formula:

[0020]

[0021] In the formula, x represents the pixel coordinates in the window neighborhood, L(x) represents the probability density corresponding to each pixel, n represents the variable dimension, Σ represents the covariance of the pixels in the window, and x i Represents the coordinates of the current center pixel, and T represents transposition;

[0022] The weighted distance d is obtained by multiplying the depth difference between the current target pixel and each neighboring pixel by the probability corresponding to the neighboring pixel. ij , sort all pixels in the window according to distance and take the first K nearest pixels;

[0023] Calculate the semantic probability distribution C of the current pixel based on the obtained K neighboring pixels i , the calculation method is as follows:

[0024]

[0025] In the formula, w ij represents the probability weight of the neighboring point relative to the current point, d represents the preset distance threshold, and C j Represents the semantic probability distribution of neighboring points, that is, the probability that the pixel belongs to each semantic category;

[0026] The calculated semantic probability distribution is normalized and the category with the highest probability is taken as the semantic category of the current pixel, as shown in the following formula:

[0027]

[0028] In the formula, c i represents the normalized semantic probability of each pixel, and m represents the semantic category.

[0029] In the above scheme, semantic segmentation is performed on the three-dimensional point cloud of the parking area scene, and point cloud distortion processing is also included as follows:

[0030] Determine the two IMU times at the acquisition point to satisfy t k <t<tk+1 ; where t is the time of the acquisition point, t k and t k+1 It is two adjacent times of IMU, corresponding to two consecutive IMU postures;

[0031] The pose translation p at the time of acquisition is calculated by the following formula: t and the rotation q t :

[0032] p t =p k +λ(p k+1 -p k )

[0033]

[0034] In the formula, λ represents the proportionality coefficient, p k and p k+1 They represent the position translation corresponding to the two IMU moments, q k and q k+1 represents the corresponding rotation amount, θ represents q k and q k+1 The angle between the vectors corresponding to the two quaternions;

[0035] After obtaining the pose T of the acquisition point at time t, convert it to the map coordinate system, taking the coordinate system of the first point collected in each frame of point cloud as a reference, as follows:

[0036] p'=Tp

[0037] Where p and p' are the homogeneous coordinates of the acquisition points before and after the transformation, respectively. The same operation is performed on all acquisition points to obtain the dedistorted point cloud.

[0038] In the above scheme, corner points and plane points are extracted based on curvature, including:

[0039] Order the point cloud and calculate the row and column index of each point; for each point in the current frame, select several left and right adjacent points on the same line bundle, and determine the curvature of the current point by the difference in depth values. The calculation formula is as follows:

[0040]

[0041] In the formula, c i is the curvature of the i-th point in the current frame; S is the set of adjacent points, j represents the j-th adjacent point; r i and r j They are the depth values ​​of the current point and the adjacent point, that is, the distance to the origin of the coordinate system;

[0042] Corner points and plane points are extracted according to the curvature; corner points are boundary points with larger curvature, and plane points are points with smaller curvature. Specifically, the points on each line bundle are divided into N segments, and all the points on each segment are arranged in order from small to large curvatures. The curvature thresholds of corner points and plane points are set to c e ,c p , select at most n points from the points with curvature greater than the corner threshold e points, select at most n points from the points whose curvature is smaller than the plane point threshold p The number of points is determined by the number of segments N and the horizontal angle resolution.

[0043] In the above scheme, a local sub-graph is constructed based on the historical key frame point cloud, and a semantically weighted line-surface matching residual between the current frame point cloud and the local sub-graph is constructed based on the corner points and plane points of the current frame point cloud, including:

[0044] When the current frame point cloud arrives, first build a local subgraph based on the historical key frame point cloud; get the initial posture of the current moment from the positioning result of the previous moment and the inter-frame pose change obtained by IMU, build a KD tree based on the positions of all historical key frames, add the key frames whose distance to the current frame is less than the threshold ζ to the local subgraph, and also add the key frames within the time range t to the local subgraph;

[0045] The local sub-image is downsampled using the voxel downsampling method, and different voxel grid sizes are used for different semantic categories;

[0046] Set the constructed local sub-image to M, and get the corner point set F of the current frame e and the plane point set F p , and at the same time, find the respective matching point pairs according to the semantic labels for matching, as follows:

[0047] For any corner point with semantic category l in the current frame In the local sub-graph, take the corner points of category l to form a set satisfy M e is the set of local sub-image corner points, and L is all semantic categories;

[0048] By constructing a KD tree Find the distance The nearest N e points, and then fit the straight line and determine whether the line characteristics are met based on the residual. If the conditions are met, the residual between the points and the line is calculated as follows:

[0049]

[0050] In the formula, represents the residual between points and lines, and represents the points on the fitted straight line;

[0051] Find the point in the local subgraph plane that matches the current point N with the same semantic category and the closest distance p If points form a plane feature, the residual between the points and the plane is calculated as follows:

[0052]

[0053] In the formula, Represents the residual between points and surfaces, and represents the points on the fitting plane;

[0054] The same operation is performed on all points in the current frame and the residuals are weighted according to their semantic categories. Finally, the overall residual RES of the corner points and plane points is obtained as follows:

[0055]

[0056] Where w is the corresponding weight, i represents the i-th corner point, and j represents the j-th plane point.

[0057] In the above scheme, the fitting shore surface of the current frame is predicted according to the fitting shore surface of the previous frame and the incremental pose between the point cloud of the current frame and the point cloud of the previous frame, including:

[0058] The plane equation of the fitted shore surface is expressed in Hesse form, that is, the plane unit normal vector n and the distance d from the origin to the plane; assuming p t-1 is a shore surface at time t-1 Points on the same, expressed in homogeneous coordinates, p' t are the corresponding points on the plane after transformation, is the plane after transformation, then:

[0059]

[0060] In the formula, T represents transpose;

[0061] According to the incremental pose T between the two frames of point cloud Δ , there is the following relationship:

[0062] p t-1 =T Δ p' t

[0063] According to the above formula, the plane equation of the predicted fitting shore surface is obtained

[0064] Associate all shore points of the current frame to the corresponding prediction plane, that is, determine the inliers corresponding to each plane. The inliers are calculated by the distance from the point to the plane. Continue to perform iterative PCA fitting on the initial inliers corresponding to each plane, and calculate the mean and covariance matrix, as shown in the following formula:

[0065]

[0066] In the formula, μ represents the mean, C represents the covariance matrix, n represents the total number of initial internal points, and p t ,i represents the i-th initial interior point at time t;

[0067] Perform eigenvalue decomposition on the covariance matrix C to obtain the corresponding eigenvalues ​​λ1<λ2<λ3 and the corresponding eigenvectors v1, v2, v3; if λ2>aλ1 is satisfied, it is considered to constitute a plane feature, where a is a proportional coefficient greater than 1, and the unit eigenvector v1 corresponding to the minimum eigenvalue is the plane normal vector;

[0068] Recalculate the internal points and update the equations according to the above steps, and obtain the final fitting shore surface of the current frame through iterative optimization;

[0069] For points that are not associated with the predicted plane and points that are determined to be outliers in the PCA iteration, these points are divided into noise points and newly appeared points according to the distance to the plane obtained after iterative optimization. If the distance is greater than the threshold, it is a new point, otherwise it is a noise point; the newly appeared points are added to the calculation, and the noise points are removed.

[0070] In the above scheme, extracting a sub-graph to fit the shore surface based on the local sub-graph includes:

[0071] The fitted shore surface of the local subgraph at the current moment is the result of the previous moment;

[0072] When any of the following two rules is met, the fitting shore surface of the local subgraph is updated:

[0073] (1) The distance traveled by the unmanned ship exceeds the threshold s since the last sub-graph fitting shore surface update;

[0074] (2) The global map is updated, which includes two situations: one is that a loop is detected, and the other is that global positioning information is added for optimization;

[0075] Construct the plane matching residual between the fitted shore surface of the current frame and the fitted shore surface of the sub-image, including:

[0076] Convert the fitted shore surface of the current frame to the map coordinate system as follows:

[0077]

[0078] Where, T wlRepresents the position relationship between the world coordinate system and the radar coordinate system, π l Represents the plane equation of the current frame in its own radar coordinate system, π l 'Represents the plane equation transformed to the map coordinate system;

[0079] Fitting the subgraph to the shore surface and To associate; if d w >0, then let π w =-π w , for π l 'Perform the same operation; the shore plane pair fitted by the normal vector and the distance from the origin to the plane is associated when the following formula is satisfied:

[0080]

[0081] |d w -d l' | <D0

[0082] In the formula, represents the unit normal vector of the sub-graph fitting shore surface, d w Indicates the distance from the origin to the sub-graph fitting shore surface, Represents the unit normal vector of the fitted shore surface transformed to the current frame in the map coordinate system, d l' It represents the distance from the origin to the fitted shore surface of the current frame in the map coordinate system, θ0 represents the angle threshold, and D0 represents the distance threshold;

[0083] Construct the residual E of all matching pairs of fitted shore planes p , as follows:

[0084]

[0085] For each pair of matching planes, weight them according to the number of inliers on the plane, where N i Indicates the number of inliers on the current frame plane.

[0086] In the above scheme, the semantic weighted line-surface matching residual and the fitting-prediction-based shore-surface matching residual constitute the semantic laser odometry factor. The overall factor graph is constructed by fusing the semantic laser odometry factor with at least one of the IMU pre-integration factor, the closed-loop detection factor, and the GPS factor to optimize the optimal global pose of the unmanned ship, including:

[0087] The overall factor graph is constructed by fusing the semantic laser odometry factor, IMU pre-integration factor, loop closure detection factor and GPS factor; where:

[0088] (1) The semantic laser odometry factor is composed of the line-surface matching residual based on semantic weighting and the shore-surface matching residual based on fitting prediction. The line-surface matching residual based on semantic weighting and the shore-surface matching residual based on fitting prediction and their corresponding Jacobians are added to the LM algorithm to optimize and solve the optimal pose, as follows:

[0089] First, calculate the Jacobian matrix of the residual of each point to be solved for the variable posture. According to the chain derivative rule, it is divided into two parts as follows:

[0090]

[0091] Among them, d is the residual, X is the pose to be solved, and p m is the coordinates of the feature points in the current frame after conversion to the map coordinate system, including corner points and plane points; the residual d is rewritten as follows, representing the distance from the point to the corresponding line or plane, p c is the projection point.

[0092]

[0093] P m The derivative is the unit vector perpendicular to the line or plane. Item, split the pose into rotation and translation, the rotation is represented by Euler angle, so p m Rewrite it as follows:

[0094] p m =R x R y R z p+t

[0095] In the above formula, p represents the feature point in the current frame, t is the translation of the current pose, and R x ,R y ,R z The rotation matrices corresponding to the Euler angles around the three axes are as follows:

[0096]

[0097] Where α, β, γ represent the Euler angles, and p m Rewrite the formula and take the derivative of the translation and Euler angle to get Finally, we get the Jacobian J of the residual to the posture; then we bring the residual and Jacobian into the LM optimization update equation as follows:

[0098] ΔX=-(JJ T +λdiag(JJ T )) -1 Jf(X)

[0099] X=X+ΔX

[0100] In the formula, λ is the damping factor, f(X) is the residual, diag() is the diagonal matrix, and ΔX is the optimized pose increment. In order to speed up the convergence of the algorithm and prevent it from falling into the local optimum, an indicator ρ is defined to characterize the degree of approximation. The calculation method is as follows:

[0101]

[0102] When the value of ρ is small, the optimization radius is increased, otherwise the radius is reduced. When the pose increment of a single-step iteration is less than the set threshold, the algorithm is considered to have converged, and the optimal pose is obtained as the pose calculation result of the semantic laser odometer.

[0103] (2) IMU pre-integration factor, which is the pose constraint obtained by integrating the IMU data between adjacent key frames;

[0104] (3) The construction of the loop detection factor is based on the Euclidean distance judgment. That is, whether the Euclidean distance between the current frame and the historical key frame position is less than the threshold is used to determine whether a loop point appears. Then, the ICP algorithm is used to match the current frame with the local subgraph constructed by the loop closure to obtain the pose constraint of the current frame and add it to the factor graph.

[0105] (4) GPS factor construction is to obtain the positioning results by combining inertial navigation, find the closest frame among all key frames, and judge whether the covariance of the positioning result of this frame is greater than the set threshold. If it is greater than the threshold, it is considered that the positioning result error is large, and the GPS positioning result is added to the factor graph as the prior information of the node.

[0106] In the above scheme, the high-precision semantic map includes: water information perception layer, shore static POI semantic layer, dynamic perception target semantic layer and berthing area function layer;

[0107] The water area information perception layer is used to perceive the scene information and hydrological and meteorological information of the berth area;

[0108] The shore static POI semantic layer is used to perceive static points of interest around the shore of the anchorage area;

[0109] Dynamic perception target semantic layer, including multi-sensor data fusion, multi-target dynamic tracking and dynamic target trajectory prediction;

[0110] The berthing area functional layer includes berth selection and berthing area management; berth selection is used to provide berthing prediction and behavioral decision-making for the berthing of unmanned ships based on existing information and real-time perception results; berthing area management is used to build a digital twin system, which collects the perception information of the environment and dynamic targets obtained by multiple sensors, and builds a digital twin model of the unmanned ship and other dynamic ships in the berthing area that combines virtual and real elements. Numerical calculations are used to simulate the dynamic performance of the ship under different conditions and reconstruct the real-time berthing environment scene.

[0111] According to a second aspect of the present invention, a method for selecting an unmanned ship berth is provided. The method for selecting an unmanned ship berth is based on the above-mentioned unmanned ship positioning method based on a high-precision semantic map, and comprises:

[0112] Taking the current position of the unmanned ship as the center, set the parking area range threshold D to divide the parking area in the map;

[0113] In the mooring area, all static ships moored at the shore are extracted based on the target tracking results and the information of each shore surface contained in the map. Whether the ships are moored at the shore is determined by the distance between the center of the ship tracking frame and the shore surface.

[0114] According to the distance from the current unmanned ship, the statically docked ship with the shortest distance is selected as a reference, and the azimuth between the docked ship and the unmanned ship itself is calculated. If the azimuth is greater than θ0, it means that the position is significantly different from the driving direction of the unmanned ship. Continue to judge whether the position of the next docked ship meets the conditions. If it does, then use the position of the ship as a reference, expand outward along the shoreline direction, and select n candidate berths in each direction;

[0115] Assume that the length and width of the bounding box of the docked ship are l1 and w1 respectively, the center coordinate is p, the distance from the berthing shore is s, the length and width of the berth are l2 and w2 respectively, the distance between different berths is d1, and the distance between the berth boundary and the shore is d2; according to the shore equation where the ship is docked, it is projected onto a two-dimensional plane to obtain the normalized direction vector v of the shoreline d and the normal vector v n ;

[0116] The coordinates p of the berth center around the docked ship b The calculation method is as follows:

[0117]

[0118] The remaining berth centers are calculated similarly;

[0119] After obtaining the berth center and berth direction, the four corner points p1, p2, p3, and p4 of the berth are further calculated according to the berth size;

[0120] The berth screening conditions are based on the berth azimuth, distance and the presence of other objects in the berth, including:

[0121] First, remove the berths whose center distance exceeds the docking range, and then calculate the azimuth of the berth. If the distance and azimuth are large, it means that the berth has a large deviation from the current driving direction of the unmanned ship. Remove the berth, and further determine whether there are other objects in the berth that meets the distance and azimuth.

[0122] The judgment method is: traverse the downsampled 3D semantic point cloud map and judge whether each point is in the berth area. In this process, the points with the semantic category of water surface are skipped. If the following conditions are met at the same time, the point is in the berth:

[0123]

[0124] In the formula, q i represents the i-th point in the point cloud, p1, p2, p3, p4 are the four corner points of the berth in counterclockwise order; count the number of points falling into the berth, and if it is greater than the threshold, remove the berth; repeat the above process during the movement of the unmanned ship to update the berth;

[0125] During the autonomous berthing process of the unmanned ship, in addition to knowing the position of the berth relative to the unmanned ship itself, it is also necessary to know its own berthing speed and angular velocity; the speed and angular velocity of the unmanned ship are calculated based on the positioning results, and the formula is as follows:

[0126]

[0127] In the formula, v t and ω t Respectively represent the speed and angular velocity at the current moment, p t ,p t-1 is the position at the current time t and the previous time, Δt is the time interval between two adjacent frames of laser point cloud, is the yaw angle

[0128] In general, the above technical solutions conceived by the present invention can achieve the following beneficial effects compared with the prior art:

[0129] (1) High-precision map architecture based on parking area semantic information and environmental information

[0130] Traditionally, electronic river maps are usually based on existing surveying and mapping data, with low update frequency, and cannot timely reflect rapidly changing environmental conditions, such as water level changes, floating objects and obstacles. They mainly provide terrain and landform information, lack key location information in berthing areas, and cannot meet the needs of unmanned ships for understanding and making decisions in complex environments. Although the accuracy of electronic river maps meets the basic needs of navigation to a certain extent, it is often insufficient to meet the requirements of high-precision unmanned ship navigation, especially in narrow or complex waters. High-precision maps that integrate semantic information and environmental information can help unmanned ships to operate and make decisions better, such as autonomous berthing and obstacle avoidance in berthing areas; combined with its real-time or high-frequency updates, it can timely reflect environmental changes, such as dynamic information such as tides, water levels, obstacles, etc., to ensure that unmanned ships can safely navigate in complex and changing environments. By integrating detailed environmental information, such as water flow speed, wind speed, weather conditions, etc., it can also provide support for the navigation planning and path optimization of unmanned ships, and enhance the environmental perception and adaptability of unmanned ships.

[0131] (2) Laser SLAM integrating 3D semantics of the mooring area and shore surface information

[0132] Generally, most SLAM algorithms are applied to road or aerial scenes of unmanned vehicles and robots. They are difficult to be directly applied in water scenes in berthing areas due to the small number of feature points. Using only geometric features without considering the semantic information of different objects will cause many mismatches. According to the specific berthing area scene, the structured features are added to the odometer optimization through weighted matching of semantic information and the constraints constructed by fitting the predicted shore surface. This can be better added as pose reference information to factor graph optimization, realizing real-time and high-precision mapping and positioning of unmanned ships during navigation and berthing.

[0133] (3) Berth selection method based on berth scene target perception

[0134] Combining the real-time update capability of dynamic semantic high-precision maps and the multi-target tracking of berths through lidar perception effects, berth selection can be better achieved. Its strategy is based on the environmental information and risk warnings obtained by ships in complex and changing port environments, and comprehensively evaluates environmental risks and ship status to make adaptive adjustments and select safer berths. With the existing berth area geometry and dynamic semantic information provided in real time, the berth space can be fully utilized for docking. BRIEF DESCRIPTION OF THE DRAWINGS

[0135] Figure 1 It is a schematic diagram of the unmanned ship positioning method based on high-precision semantic map;

[0136] Figure 2 This is the overall architecture diagram of the high-precision semantic map of the parking area;

[0137] Figure 3This is a schematic diagram of external parameter calibration;

[0138] Figure 4 This is the architecture diagram of the semantic segmentation network;

[0139] Figure 5 Construct a flow chart for shore surface constraints;

[0140] Figure 6 It is a schematic diagram of plane fitting based on prediction;

[0141] Figure 7 It is a schematic diagram of the factor graph structure;

[0142] Figure 8 Schematic diagram of the berth selection. DETAILED DESCRIPTION

[0143] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0144] At present, SLAM technology has been widely used in unmanned driving, drones, etc., but relatively less in unmanned ships. Traditional unmanned ships mostly use GPS for positioning and navigation. By summarizing and analyzing the current research status of unmanned ship perception positioning and SLAM, the shortcomings and solutions are as follows:

[0145] 1. Most maps used on the water are mainly electronic river maps, which have low accuracy and poor environmental representation capabilities. They cannot obtain detailed three-dimensional spatial information of the environment, and unmanned ships cannot obtain information about dynamic ships around them, and make timely decisions on collision avoidance and planning, which cannot meet the needs of autonomous berthing. This application designs an overall high-precision map architecture that integrates berthing area semantic information and scene information, which can obtain dynamic semantic information of the surrounding scenes, and provide and display berths on the map based on the constructed static map and perception positioning results and dynamically update them. The berths can be mapped to the map coordinate system to achieve safe, accurate and efficient autonomous berthing operations.

[0146] 2. Most of the current SLAM algorithms are applied to unmanned vehicles or robots. In the water environment of autonomous berthing, there are relatively fewer feature points and the accuracy is reduced, which makes it difficult to directly apply to unmanned ships. The shore or shoreline is an important reference information in the berthing process, which is not considered by the current method. Secondly, most SLAM algorithms only use geometric features and do not consider the semantic information of different objects. There are many mismatches, which affects the accuracy. Therefore, this application designs a laser SLAM method that integrates the semantics of the berthing area and the constraints of the shore information. By extracting structured static line and surface feature points, a semantically weighted residual is constructed, and constraints are constructed with the shore fitting features. A nonlinear optimization method is used to jointly optimize all residual terms, and then the dynamic semantic map is optimized using factor graph optimization, which solves the problem of real-time high-precision mapping and positioning for unmanned ships.

[0147] 3. At present, it is difficult for ships to make reasonable berth selection in time due to the complexity of dynamic scenes, loss of positioning signals, and real-time and changeable behavior decisions when berthing. Therefore, this application is based on the constructed dynamic semantic high-precision map and dynamic multi-target tracking of the berthing scene based on lidar perception, and designs a berth discrimination and selection method that combines scene geometric structure information and dynamic target information, which can improve the accuracy and real-time performance of berthing operations, while avoiding possible dynamic obstacles.

[0148] The present invention proposes a method for constructing and positioning a high-precision semantic map of an unmanned ship and a method for selecting a berth for a berthing scenario. Figure 1 As shown, it mainly includes:

[0149] S1. Overall architecture of high-precision semantic map of parking area

[0150] like Figure 2 As shown in the figure, the composition of the high-precision semantic map based on the berthing scene is divided into a water information perception layer, a shore static POI semantic layer, a dynamic perception target semantic layer, and a berthing area function layer.

[0151] S11 Water area information perception layer

[0152] The water area information perception layer mainly perceives the scene information and hydrological and meteorological information of the berthing area, including the identification and acquisition of information such as the 3D structure of the berthing area, the passable area, and the navigation signs on the water. When the unmanned boat is driving in the berthing area, it needs to consider the complex water conditions, such as water depth, temperature, climate, and obstacles in the waterway. These comprehensive environmental perception information are integrated into the high-precision map to ensure the safety and reliability of the unmanned boat's operation path.

[0153] S12. Static POI semantic layer on the shore

[0154] The main function of the shore static POI semantic layer is to detect and perceive static points of interest around the shore of the anchorage area, such as trees, buildings, large port equipment, etc. By acquiring perception information and performing collection processing and feature extraction, the shore surface, shore, and on-shore targets are semantically segmented as the basis for subsequent laser odometer static constraint information.

[0155] S13. Dynamic target-aware semantic layer

[0156] The dynamic target perception semantic layer functions include multi-sensor data fusion, multi-target dynamic tracking, dynamic target trajectory prediction, etc. It provides data support for the construction of dynamic semantic maps in complex and changeable berthing scenarios.

[0157] 1. Multi-sensor data fusion

[0158] For other ships operating on the water, pedestrians on the shore, and equipment operating on the shore, dynamic perception is carried out in combination with multi-sensor equipment, such as lidar, IMU, and combined inertial navigation, and the collected relevant information is fused and processed to lay the foundation for subsequent positioning and mapping functions.

[0159] 2. Multi-target dynamic tracking

[0160] It is mainly used to track target ships during encounters, obtain information that may interfere with the ship's berthing operations, and ensure safe driving during berthing. Combined with modern intelligent sensing technology, it can provide more efficient algorithm support for the perception and decision-making of ship berthing and unberthing operations.

[0161] 3. Dynamic target trajectory prediction

[0162] During the target tracking process, the historical movement trajectory of the target is recorded, and the characteristics of the trajectory are extracted and modeled, including the characteristics of speed and acceleration changes. Based on the channel intersection area and berthing topological relationship characteristics extracted from the high-precision map, combined with the unmanned ship's own spatial distribution and position relationship and environmental information, a prediction model is established, and then multi-mode prediction is performed to generate multiple future trajectories of the target under various possible constraints, and then the model is continuously updated to improve the prediction accuracy.

[0163] S14. Mooring area functional layer

[0164] The berth selection and management of the unmanned ship during the berthing process rely on the realization of its functional layer. The berthing area scene map constructed by the perception of the above static scenes and dynamic targets ultimately serves the berthing.

[0165] 1. Berth selection

[0166] The berth selection of the high-precision map berthing area functional layer provides berth prediction and behavior decision-making for the berthing of unmanned ships based on existing information and real-time perception results. Since the uncertainty of the movement of ships entering and leaving the berthing area will bring about real-time changes in berths, the process needs to fully consider the fusion of multi-source data on the ship side and the shore, and at the same time combine the ship's own factors and berthing efficiency to accurately select an optimal berth to help the ship determine a safe and efficient automatic berthing route.

[0167] 2. Mooring area management

[0168] In order to achieve overall management of the berthing area, a digital twin system is built using high-performance equipment. The system collects the perception information of the environment and dynamic targets obtained by multiple sensors, constructs a digital twin model of the unmanned ship and other dynamic ships in the berthing area that combines virtual and real elements, and uses numerical calculations to simulate the dynamic performance of the ship under different conditions, and reconstructs the real-time berthing environment scene.

[0169] S2. Construction of perception carrier for mooring area map construction

[0170] S21. Multi-sensor fusion architecture for unmanned ships

[0171] The sensors used in this application mainly include the RS80 linear mechanical laser radar of Suteng, the LPMS-IG1RS232 inertial measurement unit of Arubi, and the NPOS combined inertial navigation system of Beidou Xingtong. The host is the NVIDIA Orin development board, which is mainly used for data collection and processing, and can communicate with the lower-level Raspberry Pi through the serial port. The upper-level perception system is mainly responsible for the acquisition and processing of sensor data and the real-time operation of related algorithms. This part of the software architecture is based on the Robot Operating System (ROS). ROS has a point-to-point distributed communication mechanism to disperse the real-time computing pressure brought by different devices. The upper layer can send decision-making planning information to the lower-level control execution unit through the serial port.

[0172] S22. Calibration of multi-sensor fusion for unmanned ships

[0173] S221. LiDAR and IMU Calibration

[0174] 1. Internal reference calibration

[0175] This application adopts the Allan variance method to calibrate the internal parameters of the IMU, specifically: place and fix the IMU horizontally, collect IMU output data for about two hours, and then calculate the Allan variance and draw a double logarithmic curve of the variance changing with time, that is, the variance curve of its accelerometer and gyroscope.

[0176] 2. External parameter calibration

[0177] Calibrate the external parameters of the LiDAR and IMU, that is, before data fusion, the data collected by different sensors need to be unified to the same coordinate system. IMU is a passive sensor that cannot actively detect the environment and can only obtain carrier motion information. This application uses the hand-eye calibration method in the non-common view area method, such as Figure 3 shown.

[0178] (1) The laser radar and IMU are fixed on the carrier, and their relative posture transformation relationship remains unchanged, satisfying T1=T4. When the carrier moves, the laser radar and IMU have their own posture increments, namely T3 and T2, respectively. At this time, we have:

[0179] T LI T2=T3T LI

[0180] Where, T LI is the transformation relationship from the lidar coordinate system to the IMU coordinate system.

[0181] (2) For the pose transformation T3 between the laser radars, this application solves it through ICP matching. For the corresponding point sets P and Q in the two frame point clouds, it satisfies:

[0182] P={p1,p2,…,p n}

[0183] Q={q1,q2,…,q n}

[0184] (3) where p i ,q i is the corresponding point in the two point sets. The point with the closest spatial distance in the matching is the corresponding point. The pose transformation is to solve the rotation and translation R,t, which satisfies the following conditions:

[0185]

[0186] The matrix decomposition method is used to solve the above equation, and the source point cloud P is further transformed, and the corresponding points are re-determined. The operation is repeated. When the difference between the two posture transformations is small enough, it can be regarded as the optimal result.

[0187] (4) The posture increment T2 is obtained by integrating the measured data. This application constructs a least squares problem by measuring multiple sets of values ​​and represents the posture by Lie algebra. The formula is as follows:

[0188]

[0189] In the formula and They represent the position increments of the LiDAR and IMU from time k to k+1, and d represents the distance between the Lie algebras represented by the two position increments in the brackets. Finally, the Gauss-Newton nonlinear optimization method is used to obtain the external parameter results.

[0190] S222. Map coordinate system and GPS coordinate system calibration

[0191] Since the true value information collected by the combined inertial navigation is in the global coordinate system, and the position and posture obtained by the SLAM method is relative to the local map coordinate system, this application uses the combined inertial navigation data as the reference true value. At the same time, in order to calculate the error, it is still necessary to unify the coordinate system and calculate the position and posture transformation of the two coordinate systems.

[0192] The calibration method of combined inertial navigation and laser radar is the same as that of laser radar and IMU. The external parameter T GL Finally, the laser radar positioning result at the current moment is converted to the combined inertial navigation coordinate system. The conversion formula is as follows:

[0193]

[0194] In the formula is the position and posture of the laser radar positioning result at time k converted to the global GPS coordinate system, Indicates the true value of the combined inertial navigation positioning corresponding to the first frame of LiDAR. In practice, it is necessary to ensure that the timestamps are aligned to avoid matching errors that lead to large deviations in the overall results. GL That is, the external parameter of the combined inertial navigation to the laser radar coordinate system, is the positioning result in the map coordinate system at time k.

[0195] S3. Laser SLAM with integrated parking area semantic information

[0196] S31. Preprocessing of laser point cloud in parking area

[0197] S311. Semantic segmentation of parking area scene point cloud

[0198] 1. Semantic Segmentation Network Architecture

[0199] This application performs semantic segmentation on 3D point clouds based on the RangeNet++ network. The network can receive matrices of any size and take projected images as input. The network is mainly divided into two parts, the encoder and the decoder. The network structure is as follows: Figure 4 shown.

[0200] The downsampling rate selected in this embodiment is 32 times, and the backbone network uses Darknet53 for feature extraction, which combines the advantages of the residual network Resnet, can ensure strong feature extraction capabilities while avoiding gradient problems caused by excessive network depth.

[0201] 2. Semantic Segmentation Post-Processing

[0202] The fully convolutional semantic segmentation network can effectively extract local features of the image, but lacks the extraction of global features. This application performs post-processing based on neighborhood search on each pixel of the output depth image to improve the segmentation accuracy. Specifically:

[0203] (1) For each pixel in the depth image, take it as the center point and construct a window of size l×l. Use the current pixel coordinate as the center point to construct a Gaussian probability density function, and bring each pixel in the window into the function to obtain the probability density of the corresponding pixel. This is as follows:

[0204]

[0205] In the formula, x represents the pixel coordinates in the window neighborhood, L(x) represents the probability density corresponding to each pixel, n represents the variable dimension, Σ represents the covariance of the pixels in the window, and x i Represents the coordinates of the current center pixel, and T represents transposition.

[0206] (2) Multiply the depth difference between the current target pixel and each neighboring pixel by the probability corresponding to the neighboring pixel to obtain the weighted distance d ij , sort all the pixels in the window according to the distance, and take the first K nearest pixels, which is the result of KNN (K-Nearest Neighbors, K-nearest neighbor algorithm).

[0207] (3) Further, the semantic probability distribution of the current pixel is calculated based on the obtained K neighboring pixels. The specific calculation method is as follows:

[0208]

[0209] In the formula, w ij represents the probability weight of the neighboring point relative to the current point, d represents the preset distance threshold, and C j Represents the semantic probability distribution of neighboring points, that is, the probability that the pixel belongs to each semantic category.

[0210] (4) Finally, the calculated probability distribution is normalized and the category with the highest probability is taken as the semantic category of the current pixel. As shown in the following formula:

[0211]

[0212] In the formula, c i represents the normalized semantic probability of each pixel, and m represents the semantic category.

[0213] S312. Point cloud distortion processing

[0214] Based on the working mode of rotating laser radar, there will be a certain time difference between the first scanning point and the last point in a frame of point cloud. The obtained point cloud is inconsistent with the actual scene, which will cause point cloud distortion.

[0215] This application uses an IMU sensor and its high-frequency posture information to interpolate the postures of two adjacent IMUs, and performs motion compensation on each point to unify them into the same coordinate system to remove distortion.

[0216] (1) First, determine the IMU poses between which two moments the acquisition point is located, satisfying t k <t<t k+1 , t k and t k+1 are the time of IMU, and t is the time of acquisition point.

[0217] (2) For the translation, linear interpolation is directly used; for the rotation, this application uses spherical linear interpolation based on quaternions. The position translation and rotation at the time of acquisition can be calculated by the following formula:

[0218] p t =p k +λ(p k+1 -p k )

[0219]

[0220] In the formula, λ represents the proportionality coefficient, p k and p k+1 They represent the position translation corresponding to the two IMU moments, q k and q k+1 represents the corresponding rotation amount, θ represents q k and q k+1 The angle between the vectors corresponding to two quaternions.

[0221] (3) After obtaining the pose T of the acquisition point at time t, it needs to be converted to the map coordinate system. This application uses the coordinate system of the first point collected in each frame of point cloud as a reference, as follows:

[0222] p'=Tp

[0223] p, p' are the homogeneous coordinates of the collected points before and after the transformation. The same operation is performed on all collected points to obtain the dedistorted point cloud.

[0224] S32. Mooring area scene feature matching based on semantic weighting

[0225] S321 scene structured feature extraction

[0226] The number of point clouds acquired by LiDAR is large, and there is a large amount of redundant point information in the matching process, which affects the timeliness of the algorithm. Therefore, it is necessary to extract representative feature points from the complete point cloud.

[0227] Since each point has a semantic label after semantic segmentation, this application first removes noise points through semantic information, and then performs curvature-based feature extraction on the target point cloud. The target point cloud structure feature extraction is as follows:

[0228] (1) Order the point cloud and calculate the row and column index of each point, so that the adjacent points of each point in the horizontal and vertical directions can be quickly found through the index. For each point in the current frame, select several adjacent points on the same line bundle and determine the curvature of the current point by the difference in depth values. The calculation formula is as follows:

[0229]

[0230] In the formula, c i is the curvature of the i-th point in the current frame, S is the set of adjacent points, and in this application, 5 adjacent points on the left and right are selected, r i and r j They are the depth values ​​of the current point and the adjacent point, that is, the distance to the origin of the coordinate system.

[0231] (2) Further, edge points and plane points are extracted based on the curvature. Edge points are boundary points (or corner points) with large curvature, most of which are points on rods, while plane points are points with small curvature.

[0232] In order to extract feature points more evenly, the points on each line are divided into N segments. All the points on each segment are arranged in order from small to large in curvature, and the thresholds of corner points and plane points are set to c e ,c p , select at most n points from the points with curvature greater than the corner threshold e points, select at most n points from the points whose curvature is smaller than the plane point threshold p The number of points is determined by the number of segments N, i.e. the horizontal angle resolution.

[0233] S322. Feature matching based on semantic weighting

[0234] After extracting static line and surface feature points, this application proposes a line and surface matching residual based on semantic weighting. Since different semantic categories have different representation capabilities for scenes, semantic categories that appear less frequently in scenes and have highly structured features can more accurately represent scene detail information, and are given greater weights in the matching solution residual term, while semantic categories with more noise points, such as vegetation, have smaller weights. The details are as follows:

[0235] (1) When the current frame point cloud arrives, we first need to build a local subgraph based on the historical key frame point cloud. The initial pose of the current moment can be obtained from the positioning result of the previous moment and the inter-frame pose change obtained by IMU. A KD tree is built based on the positions of all historical key frames. Key frames whose distance to the current frame is less than the threshold ζ are added to the local subgraph. At the same time, considering the time domain information, key frames within the time range t are also added to the subgraph.

[0236] (2) Since the sub-image is composed of multiple key frames, the sub-image needs to be further downsampled. This application adopts an improved voxel downsampling method, using different voxel grid sizes for different semantic categories to retain small objects and semantic category information of interest to the greatest extent while reducing the amount of data.

[0237] (3) Set the constructed sub-image as M, and divide all feature points in the current frame into corner point sets F e and the plane point set F p At the same time, when matching, the respective matching point pairs are found according to the semantic labels for matching.

[0238] For any corner point with semantic category l in the current frame In the sub-graph, take the corner points of category l to form a set satisfy M e is the set of sub-image corner points, and L is all semantic categories. Find the distance The nearest N e Points, neighboring points are obtained by constructing a KD tree, and then a straight line is fitted and the residual is used to determine whether the line feature is met. If the condition is met, the residual between the point and the line is calculated as follows:

[0239]

[0240] In the formula, represents the residual between points and lines, and Represents points on the fitted line.

[0241] The calculation of the residual error of the plane point feature is similar. Find the distance from the current point in the sub-image feature point set. Semantically consistent and nearest N p If points form a plane feature, the residual between the points and the plane is calculated as follows:

[0242]

[0243] In the formula, Represents the residual between points and surfaces, and represents points on the fitted plane.

[0244] The same operation is performed on all points in the current frame and the residuals are weighted according to their semantic categories. Finally, the overall residuals of corner points and plane points are obtained as shown below, where w is the corresponding weight.

[0245]

[0246] Where w is the corresponding weight, i represents the i-th corner point, and j represents the j-th plane point.

[0247] S33. Construction of shore surface constraints based on fitting prediction

[0248] While constructing the semantic weighted feature matching residual of the mooring scene, this application takes the constraints of shore information into consideration, proposes a shore constraint construction method based on fitting prediction, and constructs the residual. The main process of shore constraint construction is as follows: Figure 5 shown.

[0249] This application combines the RANSAC algorithm and proposes a prediction-based iterative PCA shore surface fitting method, which makes full use of known results, predicts the current frame plane parameters through the prior fitting results of the previous frame and iteratively optimizes to improve efficiency. Since there is no prior information at the initial moment, the RANSAC method is used to fit the plane, and the incremental pose between two adjacent frames can be calculated through IMU observation data. The overall method steps are as follows:

[0250] (1) The plane equation is expressed in Hesse form, i.e., the plane unit normal vector n and the distance d from the origin to the plane. Assume that p t-1 is a shore surface at time t-1 Points on the same, expressed in homogeneous coordinates, p' t are the corresponding points on the plane after transformation, is the plane after transformation, then:

[0251]

[0252] According to the incremental pose between two frames, there is the following relationship:

[0253] p t-1 =Tp' t

[0254] According to the above formula, the predicted plane equation π' can be obtained t =T T π t-1 .

[0255] (2) Further, all shore points of the current frame are associated with the corresponding prediction planes, that is, the inliers corresponding to each plane are determined. The inliers calculation method is also determined by the distance from the point to the plane. In order to ensure the accuracy of plane fitting, the initial inliers corresponding to each plane are continuously iterated to perform PCA fitting, and the mean and covariance are calculated as follows:

[0256]

[0257] (3) Perform eigenvalue decomposition on the covariance matrix C to obtain the corresponding eigenvalues ​​λ1<λ2<λ3 and the corresponding eigenvectors v1, v2, v3. If λ2>aλ1 is satisfied, it is considered to constitute a plane feature, where a is a proportional coefficient greater than 1, and the unit eigenvector v1 corresponding to the minimum eigenvalue is the plane normal vector.

[0258] (4) Further, the inliers are recalculated according to the equation and the equation is updated according to the above steps to obtain a more accurate result. Since the prediction result is already relatively accurate, only a few PCA iterations are required. In this application, three iterations are set. In each round of iteration, a gradually decreasing proportional coefficient and inlier distance threshold can be set.

[0259] Taking into account the possible existence of occlusion or noise, points that are not associated with the predicted plane and points determined as outliers in the PCA iteration need to be considered separately. These points are divided into noise points and newly appeared plane points according to the distance from the plane obtained after iterative optimization. If the distance is greater than the threshold, it is a newly appeared point, otherwise it is a noise point. Figure 6 Schematic diagram of plane fitting based on prediction.

[0260] (5) The local sub-image constructed in S32 also needs to extract the shore surface. Since it is in the global map coordinate system, that is, the shore surface extracted from the sub-image corresponding to each frame is in the same coordinate system, the shore surface prediction result of the sub-image at the current moment is the result at the previous moment, and the subsequent process is consistent with the single-frame shore surface fitting.

[0261] This application sets the following two rules, and when one of the conditions is met, the shore surface information in the sub-graph is updated.

[0262] 1. The distance traveled by the unmanned ship since the last shore update of the sub-map exceeds the threshold s.

[0263] 2. The global map is updated. There are two main situations for map updates: one is that a loop is detected, and the other is that global positioning information is added for optimization, such as GPS.

[0264] (6) At this point, the shore surface plane fitting of the current frame and the sub-image is completed, and they are further matched. Since they are in different coordinate systems, the shore surface fitted in the current frame is first converted to the map coordinate system, as shown in the following formula:

[0265]

[0266] Where, T wl Represents the position relationship between the world coordinate system and the radar coordinate system, π l Represents the plane equation of the current frame in its own radar coordinate system, π l 'Represents the plane equation transformed to the map coordinate system.

[0267] Subgraph plane and If d w >0, then let π w =-π w , for π' l The same operation is performed. The present application fits the shore plane pair based on the information association of the normal vector and the distance from the origin to the plane. When the following formula is satisfied, the two planes are associated:

[0268]

[0269] |d w -d l' | <D0

[0270] In the formula, represents the unit normal vector of the sub-graph fitting shore surface, d w Indicates the distance from the origin to the sub-graph fitting shore surface, Represents the unit normal vector of the fitted shore surface transformed to the current frame in the map coordinate system, d l' It represents the distance from the origin to the fitted shore surface of the current frame in the map coordinate system, θ0 represents the angle threshold, and D0 represents the distance threshold.

[0271] (7) Further, the residuals of all matching pairs of fitted shore planes are constructed as follows:

[0272]

[0273] For each pair of matching planes, weight them according to the number of inliers on the plane, where N i Indicates the number of inliers on the current frame plane.

[0274] S34. Optimization of dynamic semantic map of parking area

[0275] In SLAM, the front-end odometer is an incremental method for estimating posture, which inevitably has cumulative errors. The map obtained only by the odometer has a lot of ghosting. Therefore, it is necessary to add loop information or positioning information provided by other sensors at the back end to optimize the global posture and obtain a globally consistent point cloud map.

[0276] This application uses factor graph as the backend optimization framework. This method is based on optimization and uses a graph model to describe the relationship between various sensor constraints and nodes, which can easily add various constraint information. Figure 7 As shown in the figure. i is the state variable, l i is a signpost, and the square represents a factor node. The following formula can be used to describe their relationship:

[0277] φ(X)∝φ(x1,l1)φ(x2,l1)φ(x3,l2)φ(x1,x2)φ(x2,x3)φ(x1)

[0278] Among them, φ(x1,l1), φ(x2,l1) and φ(x3,l2) represent observation factors, which are determined by the observation equations. φ(x1,x2) and φ(x2,x3) represent the pose constraint relationship between frames, which can be determined by the motion equations. φ(x1) represents the prior term.

[0279] This application constructs an overall factor graph by integrating semantic laser odometer factors, IMU pre-integration factors, closed-loop detection factors, and GPS factors. In order to reduce the amount of data, not every frame of point cloud corresponding to the carrier state is optimized for the factor graph, but only the nodes corresponding to the key frames are optimized.

[0280] (1) The semantic laser odometry factor is composed of semantically weighted feature matching and shore fitting constraint information.

[0281] Since there are relatively few plane structures in the scene, only plane matching pairs are used to optimize the pose constraints, which may be degraded, but it is less affected by noise and has better robustness. Therefore, the patent adds the S33 shore surface fitting prediction constraint residual and the corner point and plane point residual described in S32 and their corresponding Jacobian into the LM algorithm to optimize and solve the optimal pose, thereby improving the positioning accuracy of the unmanned ship written on the high-precision map of the berthing area. The details are as follows:

[0282] ① First calculate the Jacobian matrix of the residual of each point to be solved for the variable posture. According to the chain derivation rule, it can be divided into two parts as follows:

[0283]

[0284] Among them, d is the residual, X is the pose to be solved, and p m is the coordinates of the feature point in the current frame after conversion to the map coordinate system. The residual d can be rewritten as follows, representing the distance from the point to the corresponding line or plane, p c is the projection point.

[0285]

[0286] ② For p m The derivative is the unit vector perpendicular to the line or plane. Item, split the posture into rotation and translation, the rotation is expressed using Euler angles, so p m Rewrite it as follows:

[0287] p m =R x R y R z p+t

[0288] In the above formula, p represents the feature point in the current frame, t is the translation of the current pose, and R x ,R y ,R z The rotation matrices corresponding to the Euler angles around the three axes are as follows:

[0289]

[0290] ③ In the formula, α, β, and γ represent the Euler angles. m Rewrite the formula and take the derivative of the translation and Euler angle to get Finally, the Jacobian J of the residual to the posture can be obtained. Then the residual and Jacobian are brought into the LM optimization update equation as follows:

[0291] ΔX=-(JJ T +λdiag(JJ T )) -1 Jf(X)

[0292] X=X+ΔX

[0293] ④ In the formula, λ is the damping factor, f(X) is the residual, and the matrix is ​​semi-positive definite. Here, the initial value and optimization radius are set, and the iteration is continuously updated. diag() is the diagonal matrix, and ΔX is the optimization pose increment. In order to speed up the convergence of the algorithm and prevent it from falling into the local optimum, an indicator ρ is defined to characterize the degree of approximation. The calculation method is as follows:

[0294]

[0295] When the value of ρ is small, the optimization radius can be increased, otherwise the radius can be reduced. When the pose increment of a single-step iteration is less than the set threshold, the algorithm is considered to have converged, and the optimal pose is obtained as the pose calculation result of the semantic laser odometry.

[0296] (2) IMU pre-integration factor refers to the posture constraint obtained by integrating the IMU data between adjacent key frames.

[0297] (3) The construction of the loop detection factor is based on the Euclidean distance judgment. That is, whether the Euclidean distance between the current frame and the historical key frame position is less than the threshold is used to determine whether a loop point appears. The ICP algorithm is then used to match the current frame with the local subgraph constructed by the loop to obtain the pose constraint of the current frame and add it to the factor graph.

[0298] (4) GPS factor construction is to obtain the positioning results by combining inertial navigation, find the closest frame among all key frames, and judge whether the covariance of the positioning result of this frame is greater than the set threshold. If it is greater than the threshold, it is considered that the positioning result error is large, and the GPS positioning result is added to the factor graph as the prior information of the node.

[0299] S4. Berth selection based on berth scene target perception

[0300] 1. Multi-target tracking in the parking area scene perception area

[0301] This application predicts the trajectory and berth status of other ships by tracking multiple targets of dynamic ships in the berthing area, and provides optional berth status analysis for the unmanned ship to berth. The specific contents are as follows:

[0302] (1) Based on the results of real-time point cloud semantic segmentation and post-segmentation processing using the RangeNet++ network, the dynamic ships and static berth areas in the berthing area scene are screened.

[0303] (2) The present application clusters the extracted point cloud sets of target ships and berth areas with corresponding semantic labels using a distance-based adaptive Euclidean clustering method, constructs a KD tree based on the centers of all clusters, and searches for the nearest neighbor clusters. If the distance between the centers is less than a set threshold, they are marked as the same cluster, and then the three-dimensional bounding box of each cluster is calculated, and the center of the bounding box is used as the object center.

[0304] (3) Based on the berthing scenario, the state of the moving ship in the continuous frame is regarded as uniform linear motion. Its motion state model is constructed and a Kalman filter is designed for application in three-dimensional space. The state vector of the target detected in the current frame and the prediction vector of the previous frame are constructed to predict the state of the target.

[0305] (4) In the data association stage, a greedy algorithm is first used to match and associate the targets between the two frames based on the centroid Euclidean distance to find potential matching relationships, and then 3DIoU combined with the Hungarian algorithm is used to further match multiple targets.

[0306] (5) For target management, the status of the target that has been successfully associated and matched is updated. If it fails, the target is placed in the disappeared target set and the status update and the next round of prediction are continued. If the target object is in the disappeared target set for multiple frames, the target object is deleted and no longer participates in tracking.

[0307] 2. Berth selection based on berth scene perception

[0308] After obtaining the unmanned ship's own precise positioning results, target tracking results, and a high-precision static layer semantic map of the surrounding environment, this application dynamically selects multiple berths that can be docked in the map on this basis, and calculates the unmanned ship's berthing speed and its relative position relationship with the berth, providing accurate input information for the subsequent process of autonomous berthing. The main steps include:

[0309] (1) First, the parking area range threshold D is set with the current position of the unmanned ship as the center, and the parking area in the map is divided. The idle areas outside this range are not considered.

[0310] (2) In the berthing area, all static ships moored at the shore are extracted based on the target tracking results and the information of each shore surface contained in the map. Whether the ship is moored at the shore can be determined by the distance between the center of the ship tracking frame and the shore surface.

[0311] (3) Based on the distance from the current unmanned ship, the statically docked ship with the shortest distance is preferentially selected as a reference, and the azimuth between the docked ship and the unmanned ship itself is calculated. If the azimuth is greater than θ0, it means that the position is significantly different from the driving direction of the unmanned ship. Continue to judge whether the position of the next docked ship meets the conditions. If it does, then use the position of the ship as a reference and expand outward along the coastline direction. Select n candidate berths in each direction. In the embodiment of the present application, n is taken as 3, which can be adjusted according to the size of the berthing area to be considered.

[0312] Assume that the length and width of the bounding box of the docked ship are l1 and w1 respectively, the center coordinate is p, the distance from the berthing shore is s, the length and width of the berth are l2 and w2 respectively, the size parameters of the berth can be adjusted according to the size of the unmanned ship itself, the distance between different berths is d1, and the distance between the berth boundary and the shore is d2. According to the shore equation where the ship is docked, it is projected onto a two-dimensional plane to obtain the normalized direction vector v of the shoreline d and the normal vector v n . Figure 8 Schematic diagram of the berth selection.

[0313] (4) Berth center coordinates p around the docked ship b The calculation method is as follows:

[0314]

[0315] The calculation method for the remaining berth centers is similar. After obtaining the berth center and berth direction, the four corner points p1, p2, p3, and p4 of the berth can be further calculated according to the berth size.

[0316] (5) Since not all candidate berths are suitable for docking, screening is required. This application uses the berth azimuth, distance, and whether there are other objects in the berth as screening conditions.

[0317] First, remove the berths whose center distance exceeds the docking range, and then calculate the azimuth of the berth. If the distance and azimuth are large, it means that the berth has a large deviation from the current driving direction of the unmanned ship. The berth is removed, and for the berths that meet the distance and azimuth, further determine whether there are other objects in the location.

[0318] The judgment method is: traverse the three-dimensional semantic point cloud map after downsampling, and judge whether each point is in the berth area. In this process, skip the points whose semantic category is water surface. If the following conditions are met at the same time, the point is in the berth.

[0319]

[0320] Where q i represents the i-th point in the point cloud, and p1, p2, p3, and p4 are the four corner points of the berth in counterclockwise order. Count the number of points that fall into the berth, and if it is greater than the threshold, remove the berth. Repeat the above process during the movement of the unmanned ship to update the berth.

[0321] (6) During the autonomous berthing process of the unmanned ship, in addition to knowing the position of the berth relative to the unmanned ship itself, it is also necessary to know its own berthing speed and angular velocity. This application calculates the speed and angular velocity of the unmanned ship based on the precise positioning results obtained above. The formula is as follows:

[0322]

[0323] Where v t and ω t They represent the velocity and angular velocity at the current moment, respectively. Here we only consider the horizontal two-dimensional plane, p t ,p t-1 is the position at the current time t and the previous time, Δt is the time interval between two adjacent frames of laser point cloud, is the yaw angle.

[0324] In summary, the present invention proposes an unmanned ship positioning method and a berth selection method based on a high-precision semantic map, which brings the following technical effects:

[0325] (1) High-precision map architecture based on parking area semantic information and environmental information

[0326] Traditionally, electronic river charts are usually based on existing surveying and mapping data, with low update frequency, and cannot promptly reflect rapidly changing environmental conditions, such as water level changes, floating objects and obstacles. They mainly provide terrain and landform information, lack key location information in berthing areas, and cannot meet the needs of unmanned ships for understanding and making decisions in complex environments. Although the accuracy of electronic river charts meets the basic needs of navigation to a certain extent, it is often not enough to meet the requirements of high-precision unmanned ship navigation, especially in narrow or complex waters.

[0327] High-precision maps that integrate semantic information and environmental information can help unmanned ships to operate and make decisions better, such as autonomous berthing and unberthing, obstacle avoidance, etc.; combined with real-time or high-frequency updates, they can promptly reflect environmental changes, such as dynamic information such as tides, water levels, obstacles, etc., to ensure that unmanned ships can safely navigate in complex and changing environments. By integrating detailed environmental information such as water flow speed, wind speed, weather conditions, etc., it can also provide support for unmanned ships' navigation planning and path optimization, and enhance the environmental perception and adaptability of unmanned ships.

[0328] (2) Laser SLAM integrating 3D semantics of the mooring area and shore surface information

[0329] Generally, most SLAM algorithms are applied to road or aerial scenes of unmanned vehicles and robots. They are difficult to be directly applied in water scenes in berthing areas due to the small number of feature points. Using only geometric features without considering the semantic information of different objects will cause many mismatches. According to the specific berthing area scene, the structured features are added to the odometer optimization through weighted matching of semantic information and the constraints constructed by fitting the predicted shore surface. This can be better added as pose reference information to factor graph optimization, realizing real-time and high-precision mapping and positioning of unmanned ships during navigation and berthing.

[0330] (3) Berth selection method based on berth scene target perception

[0331] Combining the real-time update capability of dynamic semantic high-precision maps and the multi-target tracking of berths through lidar perception effects, berth selection can be better achieved. Its strategy is based on the environmental information and risk warnings obtained by ships in complex and changing port environments, and comprehensively evaluates environmental risks and ship status to make adaptive adjustments and select safer berths. With the existing berth area geometry and dynamic semantic information provided in real time, the berth space can be fully utilized for docking.

[0332] It should be noted that the size of the serial numbers of the steps in the above embodiments does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present application.

[0333] It should be pointed out that, according to the needs of implementation, the various steps / components described in this application can be split into more steps / components, and two or more steps / components or partial operations of steps / components can be combined into new steps / components to achieve the purpose of the present invention.

[0334] It will be easily understood by those skilled in the art that the above are only preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for positioning an unmanned ship based on a high-precision semantic map, characterized in that: The method includes: Perform semantic segmentation on the 3D point cloud of the parking area scene and extract corner points and plane points based on curvature; Construct a local sub-graph based on the historical key-frame point cloud, and construct a semantically weighted line-surface matching residual between the current frame point cloud and the local sub-graph based on the corner points and plane points of the current frame point cloud; Predict the fitted shore surface of the current frame based on the fitted shore surface of the previous frame and the incremental pose between the point cloud of the current frame and the point cloud of the previous frame; Extracting a sub-image fitting shore surface based on the local sub-image, and constructing a shore surface matching residual based on fitting prediction between the fitting shore surface of the current frame and the sub-image fitting shore surface; The semantically weighted line-surface matching residual and the fitting-prediction-based shore-surface matching residual constitute the semantic laser odometry factor. The overall factor graph is constructed by fusing the semantic laser odometry factor with at least one of the IMU pre-integration factor, the closed-loop detection factor and the GPS factor to optimize the optimal global pose of the unmanned ship.

2. The unmanned ship positioning method based on high-precision semantic map according to claim 1 is characterized in that: Semantic segmentation of the 3D point cloud of the parking area scene, including: The fully convolutional semantic segmentation network is used to perform semantic segmentation on the 3D point cloud of the parking area scene, with the projected image as input and the depth image as output to obtain the semantic segmentation result; Semantic segmentation of the 3D point cloud of the parking area scene also includes: Each pixel of the depth image is post-processed based on neighborhood search to further determine the semantic category of each pixel, which is used as the final semantic segmentation result, as follows: For each pixel in the depth image, take it as the center point and build a window of size l×l; use the current pixel coordinates as the center point to build a Gaussian probability density function, and bring each pixel in the window into the probability density of the corresponding pixel, as shown in the following formula: In the formula, x represents the pixel coordinates in the window neighborhood, L(x) represents the probability density corresponding to each pixel, n represents the variable dimension, Σ represents the covariance of the pixels in the window, and x i Represents the coordinates of the current center pixel, and T represents transposition; The weighted distance d is obtained by multiplying the depth difference between the current target pixel and each neighboring pixel by the probability corresponding to the neighboring pixel. ij , sort all pixels in the window according to distance and take the first K nearest pixels; Calculate the semantic probability distribution C of the current pixel based on the obtained K neighboring pixels i , the calculation method is as follows: In the formula, w ij represents the probability weight of the neighboring point relative to the current point, d represents the preset distance threshold, and C j Represents the semantic probability distribution of neighboring points, that is, the probability that the pixel belongs to each semantic category; The calculated semantic probability distribution is normalized and the category with the highest probability is taken as the semantic category of the current pixel, as shown in the following formula: In the formula, c i represents the normalized semantic probability of each pixel, and m represents the semantic category.

3. The unmanned ship positioning method based on high-precision semantic map according to claim 1 is characterized in that: The semantic segmentation of the 3D point cloud of the parking area scene also includes point cloud distortion processing, as follows: Determine the two IMU times at the acquisition point to satisfy t k <t<t k+1 ; where t is the time of the acquisition point, t k and t k+1 It is two adjacent times of IMU, corresponding to two consecutive IMU postures; The pose translation p at the time of acquisition is calculated by the following formula: t and the rotation q t : p t =p k +λ(p k+1 -p k ) In the formula, λ represents the proportionality coefficient, p k and p k+1 They represent the position translation corresponding to the two IMU moments, q k and q k+1 represents the corresponding rotation amount, θ represents q k and q k+1 The angle between the vectors corresponding to the two quaternions; After obtaining the pose T of the acquisition point at time t, convert it to the map coordinate system, taking the coordinate system of the first point collected in each frame of point cloud as a reference, as follows: p'=Tp Where p and p' are the homogeneous coordinates of the acquisition points before and after the transformation, respectively. The same operation is performed on all acquisition points to obtain the dedistorted point cloud.

4. The unmanned ship positioning method based on high-precision semantic map according to claim 1 is characterized in that: Extract corner points and plane points based on curvature, including: Order the point cloud and calculate the row and column index of each point; for each point in the current frame, select several left and right adjacent points on the same line bundle, and determine the curvature of the current point by the difference in depth values. The calculation formula is as follows: In the formula, c i is the curvature of the i-th point in the current frame; S is the set of adjacent points, j represents the j-th adjacent point; r i and r j They are the depth values ​​of the current point and the adjacent point, that is, the distance to the origin of the coordinate system; Corner points and plane points are extracted according to the curvature; corner points are boundary points with larger curvature, and plane points are points with smaller curvature. Specifically, the points on each line bundle are divided into N segments, and all the points on each segment are arranged in order from small to large curvatures. The curvature thresholds of corner points and plane points are set to c e ,c p , select at most n points from the points with curvature greater than the corner threshold e points, select at most n points from the points whose curvature is smaller than the plane point threshold p The number of points is determined by the number of segments N and the horizontal angle resolution.

5. The unmanned ship positioning method based on high-precision semantic map according to claim 1 is characterized in that: Construct a local sub-graph based on the historical key frame point cloud, and construct a semantically weighted line-surface matching residual between the current frame point cloud and the local sub-graph based on the corner points and plane points of the current frame point cloud, including: When the current frame point cloud arrives, first build a local subgraph based on the historical key frame point cloud; get the initial posture of the current moment from the positioning result of the previous moment and the inter-frame pose change obtained by IMU, build a KD tree based on the positions of all historical key frames, add the key frames whose distance to the current frame is less than the threshold ζ to the local subgraph, and also add the key frames within the time range t to the local subgraph; The local sub-image is downsampled using the voxel downsampling method, and different voxel grid sizes are used for different semantic categories; Set the constructed local sub-image to M, and get the corner point set F of the current frame e and the plane point set F p , and at the same time, find the respective matching point pairs according to the semantic labels for matching, as follows: For any corner point with semantic category l in the current frame In the local sub-graph, take the corner points of category l to form a set satisfy M e is the set of local sub-image corner points, and L is all semantic categories; By constructing a KD tree Find the distance The nearest N e points, and then fit the straight line and determine whether the line characteristics are met based on the residual. If the conditions are met, the residual between the points and the line is calculated as follows: In the formula, represents the residual between points and lines, and represents the points on the fitted straight line; Find the point in the local subgraph plane that matches the current point N with the same semantic category and the closest distance p If points form a plane feature, the residual between the points and the plane is calculated as follows: In the formula, Represents the residual between points and surfaces, and represents the points on the fitting plane; The same operation is performed on all points in the current frame and the residuals are weighted according to their semantic categories. Finally, the overall residual RES of the corner points and plane points is obtained as follows: Where w is the corresponding weight, i represents the i-th corner point, and j represents the j-th plane point.

6. The unmanned ship positioning method based on high-precision semantic map according to claim 1 is characterized in that: According to the fitted shore surface of the previous frame and the incremental pose between the point cloud of the current frame and the point cloud of the previous frame, the fitted shore surface of the current frame is predicted, including: The plane equation of the fitted shore surface is expressed in Hesse form, that is, the plane unit normal vector n and the distance d from the origin to the plane; assuming p t-1 is a shore surface at time t-1 Points on the same, expressed in homogeneous coordinates, p' t are the corresponding points on the plane after transformation, is the plane after transformation, then: In the formula, T represents transpose; According to the incremental pose T between the two frames of point cloud Δ , there is the following relationship: p t-1 =T Δ p’ t According to the above formula, the predicted plane equation of the fitting shore surface is obtained: t =T Δ T π t-1 ; Associate all shore points of the current frame to the corresponding prediction plane, that is, determine the inliers corresponding to each plane. The inliers are calculated by the distance from the point to the plane. Continue to perform iterative PCA fitting on the initial inliers corresponding to each plane, and calculate the mean and covariance matrix, as shown in the following formula: In the formula, μ represents the mean, C represents the covariance matrix, n represents the total number of initial internal points, and p t,i represents the i-th initial interior point at time t; Perform eigenvalue decomposition on the covariance matrix C to obtain the corresponding eigenvalues ​​λ1<λ2<λ3 and the corresponding eigenvectors v1, v2, v3; if λ2>aλ1 is satisfied, it is considered to constitute a plane feature, where a is a proportional coefficient greater than 1, and the unit eigenvector v1 corresponding to the minimum eigenvalue is the plane normal vector; Recalculate the internal points and update the equations according to the above steps, and obtain the final fitting shore surface of the current frame through iterative optimization; For points that are not associated with the predicted plane and points that are determined to be outliers in the PCA iteration, these points are divided into noise points and newly appeared points according to the distance to the plane obtained after iterative optimization. If the distance is greater than the threshold, it is a new point, otherwise it is a noise point; the newly appeared points are added to the calculation, and the noise points are removed.

7. The unmanned ship positioning method based on high-precision semantic map according to claim 6 is characterized in that: Extract sub-graphs based on local sub-graphs to fit the shore surface, including: The fitted shore surface of the local subgraph at the current moment is the result of the previous moment; When any of the following two rules is met, the fitting shore surface of the local subgraph is updated: (1) The distance traveled by the unmanned ship exceeds the threshold s since the last sub-graph fitting shore surface update; (2) The global map is updated, which includes two situations: one is that a loop is detected, and the other is that global positioning information is added for optimization; Construct the plane matching residual between the fitted shore surface of the current frame and the fitted shore surface of the sub-image, including: Convert the fitted shore surface of the current frame to the map coordinate system as follows: π’ l ＝T wl -T π l Where, T wl Represents the position relationship between the world coordinate system and the radar coordinate system, π l Represents the plane equation of the current frame in its own radar coordinate system, π' l Represents the plane equation transformed to the map coordinate system; Fitting the subgraph to the shore surface and To associate; if d w >0, then let π w =-π w , for π' l Perform the same operation; the shore plane pair fitted by the normal vector and the distance from the origin to the plane is associated when the following formula is satisfied: |d w -d l' |<D0 In the formula, represents the unit normal vector of the sub-graph fitting shore surface, d w Indicates the distance from the origin to the sub-graph fitting shore surface, Represents the unit normal vector of the fitted shore surface transformed to the current frame in the map coordinate system, d l' It represents the distance from the origin to the fitted shore surface of the current frame in the map coordinate system, θ0 represents the angle threshold, and D0 represents the distance threshold; Construct the residual E of all matching pairs of fitted shore planes p , as follows: For each pair of matching planes, weight them according to the number of inliers on the plane, where N i Indicates the number of inliers on the current frame plane.

8. The unmanned ship positioning method based on high-precision semantic map according to claim 1 is characterized in that: The semantic weighted line-surface matching residual and the fitting-prediction-based shore-surface matching residual constitute the semantic laser odometry factor. The overall factor graph is constructed by fusing the semantic laser odometry factor with at least one of the IMU pre-integration factor, the closed-loop detection factor, and the GPS factor to optimize the optimal global pose of the unmanned ship, including: The overall factor graph is constructed by fusing the semantic laser odometry factor, IMU pre-integration factor, loop closure detection factor and GPS factor; where: (1) The semantic laser odometry factor is composed of the line-surface matching residual based on semantic weighting and the shore-surface matching residual based on fitting prediction. The line-surface matching residual based on semantic weighting and the shore-surface matching residual based on fitting prediction and their corresponding Jacobians are added to the LM algorithm to optimize and solve the optimal pose, as follows: First, calculate the Jacobian matrix of the residual of each point to be solved for the variable posture. According to the chain derivative rule, it is divided into two parts as follows: Among them, d is the residual, X is the pose to be solved, and p m is the coordinates of the feature points in the current frame after conversion to the map coordinate system, including corner points and plane points; the residual d is rewritten as follows, representing the distance from the point to the corresponding line or plane, p c is the projection point. P m The derivative is the unit vector perpendicular to the line or plane. Item, split the pose into rotation and translation, the rotation is represented by Euler angle, so p m Rewrite it as follows: p m =R x R y R z p+t In the above formula, p represents the feature point in the current frame, t is the translation of the current pose, and R x ,R y ,R z The rotation matrices corresponding to the Euler angles around the three axes are as follows: Where α, β, γ represent the Euler angles, and p m Rewrite the formula and take the derivative of the translation and Euler angle to get Finally, we get the Jacobian J of the residual to the posture; then we bring the residual and Jacobian into the LM optimization update equation as follows: ΔX=-(JJ T +λdiag(JJ T )) -1 Jf(X) X=X+ΔX In the formula, λ is the damping factor, f(X) is the residual, diag() is the diagonal matrix, and ΔX is the optimized pose increment. In order to speed up the convergence of the algorithm and prevent it from falling into the local optimum, an indicator ρ is defined to characterize the degree of approximation. The calculation method is as follows: When the value of ρ is small, the optimization radius is increased, otherwise the radius is reduced. When the pose increment of a single-step iteration is less than the set threshold, the algorithm is considered to have converged, and the optimal pose is obtained as the pose calculation result of the semantic laser odometer. (2) IMU pre-integration factor, which is the pose constraint obtained by integrating the IMU data between adjacent key frames; (3) The construction of the loop detection factor is based on the Euclidean distance judgment. That is, whether the Euclidean distance between the current frame and the historical key frame position is less than the threshold is used to determine whether a loop point appears. Then, the ICP algorithm is used to match the current frame with the local subgraph constructed by the loop closure to obtain the pose constraint of the current frame and add it to the factor graph. (4) GPS factor construction is to obtain the positioning results by combining inertial navigation, find the closest frame among all key frames, and judge whether the covariance of the positioning result of this frame is greater than the set threshold. If it is greater than the threshold, it is considered that the positioning result error is large, and the GPS positioning result is added to the factor graph as the prior information of the node.

9. The unmanned ship positioning method based on high-precision semantic map according to claim 1 is characterized in that: The high-precision semantic map includes: water information perception layer, shore static POI semantic layer, dynamic perception target semantic layer and berthing area function layer; The water area information perception layer is used to perceive the scene information and hydrological and meteorological information of the berth area; The shore static POI semantic layer is used to perceive static points of interest around the shore of the anchorage area; Dynamic perception target semantic layer, including multi-sensor data fusion, multi-target dynamic tracking and dynamic target trajectory prediction; The berthing area functional layer includes berth selection and berthing area management; berth selection is used to provide berthing prediction and behavioral decision-making for the berthing of unmanned ships based on existing information and real-time perception results; berthing area management is used to build a digital twin system, which collects the perception information of the environment and dynamic targets obtained by multiple sensors, and builds a digital twin model of the unmanned ship and other dynamic ships in the berthing area that combines virtual and real elements. Numerical calculations are used to simulate the dynamic performance of the ship under different conditions and reconstruct the real-time berthing environment scene.

10. A method for selecting an unmanned ship berth, characterized in that: The unmanned ship berth selection method is based on the unmanned ship positioning method based on the high-precision semantic map described in claim 9, comprising: Taking the current position of the unmanned ship as the center, set the parking area range threshold D to divide the parking area in the map; In the mooring area, all static ships moored at the shore are extracted based on the target tracking results and the information of each shore surface contained in the map. Whether the ships are moored at the shore is determined by the distance between the center of the ship tracking frame and the shore surface. According to the distance from the current unmanned ship, the statically docked ship with the shortest distance is selected as a reference, and the azimuth between the docked ship and the unmanned ship itself is calculated. If the azimuth is greater than θ0, it means that the position is significantly different from the driving direction of the unmanned ship. Continue to judge whether the position of the next docked ship meets the conditions. If it does, then use the position of the ship as a reference, expand outward along the shoreline direction, and select n candidate berths in each direction; Assume that the length and width of the bounding box of the docked ship are l1 and w1 respectively, the center coordinate is p, the distance from the berthing shore is s, the length and width of the berth are l2 and w2 respectively, the distance between different berths is d1, and the distance between the berth boundary and the shore is d2; according to the shore equation where the ship is docked, it is projected onto a two-dimensional plane to obtain the normalized direction vector v of the shoreline d and the normal vector v n ; The coordinates p of the berth center around the docked ship b The calculation method is as follows: The remaining berth centers are calculated similarly; After obtaining the berth center and berth direction, the four corner points p1, p2, p3, and p4 of the berth are further calculated according to the berth size; The berth screening conditions are based on the berth azimuth, distance and the presence of other objects in the berth, including: First, remove the berths whose center distance exceeds the docking range, and then calculate the azimuth of the berth. If the distance and azimuth are large, it means that the berth has a large deviation from the current driving direction of the unmanned ship. Remove the berth, and further determine whether there are other objects in the berth that meets the distance and azimuth. The judgment method is: traverse the downsampled 3D semantic point cloud map and judge whether each point is in the berth area. In this process, the points with the semantic category of water surface are skipped. If the following conditions are met at the same time, the point is in the berth: In the formula, q i represents the i-th point in the point cloud, p1, p2, p3, p4 are the four corner points of the berth in counterclockwise order; count the number of points falling into the berth, and if it is greater than the threshold, remove the berth; repeat the above process during the movement of the unmanned ship to update the berth; During the autonomous berthing process of the unmanned ship, in addition to knowing the position of the berth relative to the unmanned ship itself, it is also necessary to know its own berthing speed and angular velocity; the speed and angular velocity of the unmanned ship are calculated based on the positioning results, and the formula is as follows: In the formula, v t and ω t Respectively represent the speed and angular velocity at the current moment, p t ,p t-1 is the position at the current time t and the previous time, Δt is the time interval between two adjacent frames of laser point cloud, is the yaw angle.

Images