An unmanned ship laser radar point cloud registration method for complex environment interference

Abstract

The application relates to the technical field of point cloud data processing, in particular to a complex environment interference-oriented unmanned ship laser radar point cloud registration method, which comprises the following steps: learning the spatial relationship of a two-dimensional image by using a Transform architecture to generate a data set; performing hypersphere learning by using the data set to remove abnormal point clouds of three-dimensional laser point clouds to obtain second three-dimensional laser point clouds; updating the state variable corresponding to each point cloud according to an SGD algorithm and the second three-dimensional laser point clouds; updating the optimal update direction of the point clouds in the second three-dimensional laser point clouds by using the gradient of a cost function, the logarithmic gradient of the translation Gaussian prior and the logarithmic gradient of the rotation von Mises prior according to STEIN variational gradient descent; and performing registration on the second three-dimensional laser point clouds through iteration until convergence is achieved, so that the position and the attitude of the unmanned ship can be accurately estimated. The method can not only be used for alignment of two frames of point clouds, but also can accurately estimate the uncertainty of an ICP pose alignment result, so that a better alignment result can be obtained.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of point cloud data processing, and particularly relates to a method for registering laser radar point clouds of an unmanned ship in a complex environment. BACKGROUND

[0002] The SLAM technology of the unmanned ship can be used to fuse virtual content with the real world to achieve a more immersive and interactive experience, but still has some deficiencies in some specific scenarios. These scenarios include but are not limited to complex terrain or structures, adverse weather conditions, occlusions, strong interference or noise, lack of features or textures, etc. In these scenarios, the SLAM system may encounter difficulties or be damaged. At present, ICP is commonly used in SLAM systems that incorporate laser radar observations. In actual applications, the sensors of the unmanned ship are easily disturbed by noise caused by water flow and sea waves during navigation, and the sensors may be affected by noise caused by raindrops, fog, crosswind or atmospheric turbulence in adverse weather conditions. Therefore, how to optimize and improve the ICP algorithm of the SLAM system of the unmanned ship or combine other technologies to optimize the ICP algorithm to eliminate the influence of noise on the unmanned ship during navigation on the water surface is a major challenge for the SLAM technology of the unmanned ship at present. Therefore, it is urgent to research a positioning algorithm for the unmanned ship in water operations that fuses multi-sensor information to improve the robustness and accuracy of the positioning system of the unmanned ship in scenarios with disturbances and adverse weather. SUMMARY

[0003] In view of the above defects, the purpose of the present application is to provide a method for registering laser radar point clouds of an unmanned ship in a complex environment, which aims to eliminate the influence of noise on the unmanned ship during navigation on the water surface and improve the robustness and accuracy of the positioning system of the unmanned ship in scenarios with disturbances and adverse weather.

[0004] To achieve this purpose, the present application adopts the following technical solutions:

[0005] A method for registering laser radar point clouds of an unmanned ship in a complex environment, comprising the steps of:

[0006] S10: converting a three-dimensional laser point cloud to be registered into a two-dimensional image, learning the spatial relationship of the two-dimensional image using a Transformer architecture, and creating a binary classification model according to the Transformer architecture;

[0007] S20: generating a data set, performing binary classification on the three-dimensional laser point cloud according to the binary classification model and the data set, and the data set including a labeled data set and an unlabeled data set;

[0008] S30: based on the anomaly detection model, hyper-sphere learning is performed on the data set to remove the abnormal point cloud of the three-dimensional laser point cloud, and a second three-dimensional laser point cloud is obtained;

[0009] S40: the state variables including the translation vector and the rotation matrix are optimized based on the initial estimation of the minimum distance;

[0010] S50: according to the nearest neighbor search, the point cloud corresponding to each source point cloud in the second three-dimensional laser point cloud is found, the cost function of ICP is optimized according to the SGD algorithm and the second three-dimensional laser point cloud, the gradient of the optimized cost function is calculated according to the SGD-ICP algorithm, and the state variable corresponding to each point cloud is updated;

[0011] S60: according to the STEIN variational gradient descent, the log gradient of the translation Gaussian prior of the second three-dimensional laser point cloud, the log gradient of the rotation von Mises prior and the optimal update direction are generated;

[0012] S70: the optimal update direction of the point cloud in the second three-dimensional laser point cloud is updated according to the gradient of the cost function, the log gradient of the translation Gaussian prior and the log gradient of the rotation von Mises prior, and the distribution of the second three-dimensional laser point cloud is updated using the point cloud update rule;

[0013] S80: iterate steps S40-S70 until convergence, realize the registration of the second three-dimensional laser point cloud, and accurately estimate the position and attitude of the unmanned ship.

[0014] Preferably, the use of the Transformer architecture to learn the spatial relationship of the two-dimensional image includes using a local attention mechanism to associate each pixel to its adjacent region, and the attention of the pixel corresponds to formula (1):

[0015]

[0016] wherein y ij represents the attention weighting sum at positions i and j, a and b represent the position index in the adjacent region respectively, is the adjacent region, x ij is a feature vector representing positions i and j, q ij = W Q x ij , k ab = W K x ab , v ab = W V x ab , and W Q , WK 、 They are all the corresponding first weight matrices, x ab is the eigenvector at position a and position b, that is, q ij is the query weight matrix W i with x ij The calculated query vector, k ab is the weight matrix W of the feature vector and key at positions a and b K The calculated key vector, v ab is the weight matrix W of the eigenvectors and values ​​at positions a and b v The calculated value vector, T is the transpose of the vector, parameter r a-i,b-i is the position code to be learned, and softmax() is the calculation function of the attention weight.

[0017] Preferably, generating a data set includes:

[0018] Collect several continuous two-dimensional lidar images before the complex environment interference, and calculate the effective measurement number of the two-dimensional lidar images before the complex environment interference as n before ;

[0019] Collect several continuous two-dimensional lidar images under complex environmental interference and calculate the effective measurement times n of the two-dimensional lidar images under complex environmental interference after ;

[0020] Marking pixels according to the effective measurement times of the two-dimensional lidar image before the complex environment interference and the effective measurement times of the two-dimensional lidar image during the complex environment interference, to generate the labeled data set and the unlabeled data set;

[0021] The labeled dataset is D n , and D n ={(x1,l1),…,(x n ,l n )}, where x n is the nth pixel of the 2D lidar image under complex environment interference, l n is the pixel label corresponding to the nth pixel, when n before -n after If it is greater than a preset value, the pixels without valid measurements in the two-dimensional lidar image under complex environment interference are marked as l = -1, and the rest are set to l = 1;

[0022] The unlabeled dataset is and in, D n The set of pixels where l = -1, representing the labeled dataset D n the mth pixel with l = -1.

[0023] Preferably, the based on the anomaly detection model, the hyper-sphere learning with the label set is specifically:

[0024] According to the objective function of the DeepSAD model, the pixels with l = 1 in the labeled dataset are adjusted to a hyper-sphere, and the objective function is shown as formula (2):

[0025]

[0026] Wherein, n is the number of pixels of the labeled dataset, m is the number of pixels of the unlabeled dataset, representing the i-th pixel with l = -1, x j representing the j-th pixel with l = 1, l j is the pixel label of the j-th pixel in the unlabeled dataset, is the weight matrix of the Transformer architecture, and is initialized using the weights of the binary classification model, and the parameter c is in a preset hyper-sphere center in the space, the parameter η' is a hyperparameter for balancing the weights of the labeled dataset and the unlabeled dataset, the parameter λ is a hyperparameter for the regularization term, and F is the Frobenius norm.

[0027] Preferably, the removing the abnormal point cloud of the three-dimensional laser point cloud to obtain a second three-dimensional laser point cloud comprises:

[0028] Calculating the distance d(x) of each point cloud in the three-dimensional laser point cloud to the preset hyper-sphere center c, and the distance d(x) is calculated as shown in formula (3):

[0029] d(x) = ‖Transformer(x; W * )-c‖ (3)

[0030] Wherein, x is a point cloud in the three-dimensional laser point cloud, W * is the weight matrix of the trained Transformer architecture, and c is a preset hyper-sphere center;

[0031] Scaling the distance of each point cloud to the preset hyper-sphere center c to obtain the abnormal score d'(x) of each point cloud in the three-dimensional laser point cloud, and the scaling is shown as formula (4):

[0032]

[0033] According to the abnormal score of each point cloud in the three-dimensional laser point cloud, the abnormal point cloud is removed.

[0034] Preferably, the step of calculating the gradient of the optimized cost function according to the SGD-ICP algorithm and updating the state variable corresponding to each point cloud comprises the steps of:

[0035] deriving the partial derivative of the state variable before updating according to the cost function, to calculate the gradient of the translation vector and the gradient of the rotation matrix;

[0036] updating the state variable according to the gradient of the translation vector and the gradient of the rotation matrix;

[0037] The cost function is shown in formula (5):

[0038]

[0039] Wherein, M is the number of samples obtained from the second three-dimensional laser point cloud , e is a point cloud in the sample , s e is a point cloud in the sample , r e is the point cloud with the shortest distance to s e , the state variable θ={x, y, z, roll, pitch, yall}, wherein the first three dimensions x, y and z of θ constitute the translation amount θ 1:3 , u is the translation vector, roll is the rotation angle around the x axis, pitch is the rotation angle around the y axis, and yall is the rotation angle around the z axis, is the last three dimensions θ 4:6 of the rotation matrix parameterized by θ;

[0040] The gradient of the translation vector is as shown in formula (6):

[0041]

[0042] The gradient of the rotation matrix is as shown in formula (7):

[0043]

[0044] Wherein, R t is the transpose of the rotation matrix, u t is the transpose of the translation vector, is the partial derivative of the translation vector with respect to the translation amount, is the partial derivative of the rotation matrix with respect to the rotation angle, and e is a point cloud in the sample ;

[0045] The updated state variable θ is shown in equation (8):

[0046]

[0047] wherein θ t and θ t+1 are the pose parameter values of the current iteration and the next iteration respectively, the matrix is the preconditioner of the gradient, and the learning rate lr determines the speed of the parameter value change, is the partial derivative of the cost function with respect to the state variable θ.

[0048] Preferably, the generating the optimal update direction of the second three-dimensional laser point cloud comprises:

[0049] calculating the logarithmic gradient of the posterior probability based on the operator of STEIN;

[0050] deriving the optimal update direction of the point cloud according to the logarithmic gradient of the posterior probability in combination with the STEIN variational gradient descent;

[0051] the operator is the operator is shown in equation (9):

[0052]

[0053] wherein, is the expectation operation, p is the intractable but differentiable posterior distribution, the function KL(·‖·) represents the KL divergence between the point cloud distribution and the target distribution, η is a small step size, is the derivative of the step size, q [ηφ] is specified non-parametrically by the updated point cloud x' = x + ηφ(x), wherein x is the un-updated point cloud, η is a small step size, φ(x) is the steepest direction, trace() is the trace operation of the matrix, the operator the expression of the product of the steepest direction is shown in equation (10):

[0054]

[0055] According to equation (9) and equation (10), the logarithmic gradient of the posterior probability is calculated:

[0056]

[0057] wherein N is the total number of pixels of the data set, is the logarithmic gradient of the posterior probability, x is the point cloud before registration, D is the data set of the second three-dimensional laser point cloud, d idx is the idxth point cloud of the second three-dimensional laser point cloud, and respectively are the results of taking the derivative of the log-likelihood function and the prior with respect to x;

[0058] The optimal update direction of the point cloud is φ * (·), φ * (·) is shown in equation (12):

[0059]

[0060] where, is the expectation operation, k(x,·) is the kernel function, x is the point cloud before update, is the gradient with respect to x, is the result of taking the derivative of the prior with respect to x.

[0061] Preferably, the result of taking the derivative of the prior with respect to x is the log-gradient of the translation Gaussian prior and the log-gradient of the rotation von Mises prior, as shown in equations (13) and (14) respectively:

[0062]

[0063]

[0064] where, is the log-gradient of the translation Gaussian prior, is the log-gradient of the rotation von Mises prior, μ 1:3 and μ 4:6 represent the mean values of translation and rotation, while σ 1:3 and represent the variances of the prior distribution of translation and rotation components respectively.

[0065] Preferably, the point cloud update rule is shown in equation (15):

[0066]

[0067] where, x is the j'th point cloud before registration, x j, is the x j, th point cloud after registration, η is a small step size, is the steepest direction, as shown in equation (16):

[0068]

[0069] where, K is the total number of point clouds of the second three-dimensional laser point cloud, is weighted by the kernel function, smooths the gradient, and serves as the steepest direction of the log-probability, is the gradient of the kernel function, x is the point cloud before the j′th registration, x j′ is the xth j′ point clouds;

[0070] Updating the optimal update direction of the point cloud in the second three-dimensional laser point cloud according to the gradient of the cost function, the logarithmic gradient of the Gaussian prior of the translation, and the logarithmic gradient of the von Mises prior of the rotation, and updating the distribution of the second three-dimensional laser point cloud using a point cloud update rule includes:

[0071] Substitute the gradient of the translation vector and the logarithmic gradient of the Gaussian prior of the translation into , obtaining the updated translation vector of the point cloud;

[0072] Substitute the gradient of the rotation matrix and the logarithmic gradient of the rotation von Mises prior into , obtaining the updated rotation matrix of the point cloud;

[0073] The optimal update direction of each point cloud is calculated according to the gradient of the cost function, the updated translation vector and the updated rotation matrix, and the steepest direction in formula (15) is substituted to update the point cloud distribution;

[0074] The optimal update direction of each point cloud is shown in formula (17):

[0075]

[0076] Where μ is the average value, K is the total number of point clouds of the second three-dimensional laser point cloud, θ j′ is the state variable of the j′th point cloud, μ and θ j The difference, is the logarithmic gradient of the state variable, is the optimal update direction for each point cloud, is the prior gradient of the state variable, k(θ j′ ,θ) is the kernel function, is the gradient of the kernel function with respect to θ.

[0077] Preferably, the kernel function includes a kernel function corresponding to translation and a kernel function corresponding to rotation;

[0078] The kernel function corresponding to the translation is shown in formula (18):

[0079]

[0080] The kernel function corresponding to the rotation is shown in formula (19):

[0081]

[0082] where, θ 1:3 The first three dimensions x, y and z of θ 1:3 are translation amounts, θ 1:3 is the solving target of θ 4:6 , θ 4:6 is the rotation angle of the x, y and z axes respectively, h is the bandwidth of the kernel function, is the square of the two-norm of the vector, arctan2() is a variant of the inverse tangent function, sin(θ 4:6 -θ′ 4:6 ) is the rotation sine value of the point cloud, cos(θ 4:6 -θ′ 4:6 ) is the rotation cosine value of the point cloud, exp() represents the natural exponential function, and k(0 represents the kernel function.

[0083] One of the technical solutions in the above technical solution has the following advantages or beneficial effects:

[0084] 1. Based on the STEIN variational inference framework, a STEIN ICP algorithm with gradient optimization is designed, which can not only be used for the alignment of two frames of point clouds, but also can accurately estimate the uncertainty of the ICP pose alignment result to achieve a better alignment result. By using a set of point clouds to approximate the difficult-to-calculate posterior distribution, and using a repulsive term to prevent point clouds from gathering together, the complex uncertainty distribution of the disturbance of the water flow, sea waves and bad weather to the sensor when the unmanned ship sails on the water surface can be captured. The influence of water surface error and uncertainty on ICP can be effectively eliminated. The influence of environmental noise and outliers on the matching result during the operation of the unmanned ship can be effectively reduced, which has the advantages of good robustness, fast convergence speed, and insensitivity to initial values.

[0085] 2. The Transformer architecture is used to capture the spatial relationship between adjacent laser measurements, and then an anomaly detection model is used to improve the distribution of normal classes by clustering normal samples into a hypersphere and mapping other abnormal samples to the center of the hypersphere. Finally, the distance of the query sample to the center of the hypersphere is calculated as its anomaly score. This method can identify whether the performance of the laser radar sensor has degraded in time, and use the degradation information to reduce the uncertainty in the environment, thereby improving the stability of the unmanned ship registration in bad weather such as heavy rain. BRIEF DESCRIPTION OF DRAWINGS

[0086] Figure 1 is a flowchart of an unmanned ship laser radar point cloud registration method for complex environmental interference of the present application;

[0087] Figure 2is a relationship diagram of a three-dimensional laser point cloud of an embodiment of a complex environment interference-oriented unmanned ship laser radar point cloud registration method of the application and its corresponding two-dimensional image;

[0088] Figure 3 is a schematic diagram of a Tranformer architecture of a complex environment interference-oriented unmanned ship laser radar point cloud registration method of the application. DETAILED DESCRIPTION

[0089] The embodiments of the present application will be described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar notations represent the same or similar elements or elements with the same or similar functions throughout. The embodiments described below by referring to the accompanying drawings are exemplary and are only used to explain the present application, and cannot be understood as a limitation of the present application.

[0090] In the description of the present application, it should be understood that the orientation or positional relationship indicated by the terms "center", "longitudinal", "transverse", "length", "width", "thickness", "upper", "lower", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", "axial", "radial", "circumferential" and the like are based on the orientation or positional relationship shown in the drawings, and are only for the convenience of describing the present application and simplifying the description, and therefore cannot be understood as indicating or implying that the devices or elements referred to must have a particular orientation, be constructed and operated in a particular orientation, and therefore cannot be understood as a limitation of the present application.

[0091] In addition, the terms "first", "second" are only for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Therefore, the features defined with "first", "second" can explicitly or implicitly include one or more of the features. In the description of the present application, unless otherwise specified and limited, the meaning of "a plurality of" is two or more.

[0092] In the description of the present application, it should be noted that, unless otherwise explicitly specified and limited, the terms "mounting", "connection", "connection" should be understood broadly, for example, it can be fixed connection, or detachable connection, or integrally connected; it can be directly connected, or indirectly connected through an intermediate medium, or the communication inside two elements.

[0093] Technical terms involved in the present application:

[0094] SGD (Stochastic Gradient Descent): SGD is an optimization algorithm used to minimize a certain loss function. It calculates the gradient by randomly selecting a sample or a small batch of samples at each iteration and updates the parameters in the opposite direction of the gradient. SGD is commonly used to train machine learning models, such as neural networks, to reduce prediction error or loss function value.

[0095] ICP (Iterative Closest Point): ICP is an iterative point cloud registration algorithm commonly used in SLAM systems that incorporate lidar observations, providing six-degree-of-freedom pose estimates and point cloud maps. The goal is to find the best transformation (usually rotation and translation) between two or more point clouds to align them as closely as possible. ICP works by iteratively finding the closest point pairs between two point clouds and computing the best transformation to minimize the distance or error between them. ICP is widely used in computer vision, robotics, and mapping, among other fields.

[0096] Stein Variational Gradient Descent (SVGD): Stein Variational Gradient Descent is an optimization algorithm primarily used for optimizing probability distributions. It is similar to Stochastic Gradient Descent (SGD), but while SGD is used to optimize function values, SVGD is used to optimize probability distributions. In Stein Variational Gradient Descent, the gradient of the particles is computed, and the particles are updated in the direction of the gradient, so that the particle distribution gradually approaches the target distribution. The key to this method is the use of the Stein operator, which can map one function to another, so that the mapped function has an expectation of 0 with respect to a certain probability distribution.

[0097] For the unmanned surface vehicle lidar point cloud registration method proposed in this patent, a rainy day environment is used as an example of a complex environment to illustrate the point cloud registration method.

[0098] As Figure 1 shown, a preferred embodiment of the present application includes the steps of:

[0099] S10: converting the three-dimensional laser point cloud to be registered into a two-dimensional image, using a Transformer architecture to learn the spatial relationship of the two-dimensional image, and creating a binary classification model based on the Transformer architecture;

[0100] The Transformer architecture is a kind of attention mechanism model, and the Transformer architecture is used to capture the spatial relationship between adjacent laser measurements. The three-dimensional laser point cloud contains a large amount of point cloud information, and by converting the three-dimensional point cloud into a two-dimensional image, a more easily processed data form is provided. A feasible way to convert is to project or render. In this embodiment, the laser measurement point cloud with coordinates (x, y, z) is projected onto a two-dimensional image plane using the projection method; and the pixel coordinates (i, j) on the image plane are determined according to the laser beam index i and the angle j. Wherein, the laser beam index i represents the order of the laser beam receiving the measurement, which is used to determine the vertical position of the pixel on the image, and the angle j represents the horizontal position of the pixel on the image. The angle j is calculated as wherein is the inverse tangent of the ratio of y to x, and the parameter φ is the angular resolution of the lidar. By projecting each laser measurement to the appropriate location on the image plane, a two-dimensional image is formed as shown in Figure 2 The angular resolution of the lidar determines the size of each pixel, which contains five values from its corresponding laser measurement: the 3D coordinates (x, y, z), the distance value and the intensity value used by the Transformer architecture to learn the lidar signal, which is summed with r as the pixel grayscale value of the 2D image. For pixels corresponding to abnormal laser measurement values, all their values are set to zero.

[0101] As shown in Figure 3 The Transformer architecture of the present embodiment is composed of a self-attention layer, a normalization layer with skip connection, a linear layer, and a normalization layer with skip connection. Using the attention mechanism of the Transformer architecture, each pixel is associated with the neighboring region, and the Transformer can learn the position information of each pixel and incorporate it into the scope of attention. In this way, the model can better understand the relationship between different positions in the image, thereby improving the ability to handle spatial relationships. The skip connection and normalization layer help to alleviate the problem of gradient vanishing during training, while improving the training speed and stability of the model. These mechanisms make the Transformer easier to train and optimize, so that it can more effectively learn the spatial relationships in the image.

[0102] A linear layer is added after the Transformer architecture, and the Transformer architecture is trained using a binary cross-entropy loss function to create a binary classification model, reducing the feature dimension from five dimensions to one dimension.

[0103] S20: generating a data set, and performing binary classification on the three-dimensional laser point cloud according to the binary classification model and the data set, wherein the data set includes a labeled data set and an unlabeled data set;

[0104] Specifically, a binary classification model is used to roughly distinguish between normal and abnormal lidar point clouds, generating a dataset of two-dimensional images corresponding to the lidar point cloud. The dataset includes a labeled dataset containing measurement data corresponding to the normal category and abnormalities, and an unlabeled dataset containing all abnormal measurement data. Both the labeled dataset and the unlabeled dataset are used to train the abnormal measurement model, providing necessary data support for subsequent model training, providing preliminary processing of lidar point cloud data, and laying the foundation for subsequent anomaly detection and registration. After generating the dataset, the binary classification model distinguishes between valid and invalid pixels from the features of the two-dimensional image. This process provides a labeled dataset for the binary classification model.

[0105] S30: Based on the anomaly detection model, use the label set for hypersphere learning, and remove the abnormal point cloud of the three-dimensional laser point cloud to be calibrated to obtain a second three-dimensional laser point cloud;

[0106] Wherein, the hypersphere learning is an anomaly detection algorithm, which constructs a hypersphere in the dataset by learning the distribution of data points, and considers the points outside the sphere as abnormal points. The anomaly detection model can automatically learn the complex patterns in the data and identify samples that do not match these patterns, thereby performing anomaly detection without explicit labels.

[0107] Specifically, first, use the unlabeled dataset to train the anomaly detection algorithm. After training, use the trained anomaly detection algorithm for hypersphere learning, so that the hypersphere can contain as much normal data as possible while excluding abnormal data. Use the learned hypersphere model to remove the abnormal point cloud of the three-dimensional laser point cloud to be registered to obtain a second three-dimensional laser point cloud, i.e. the point cloud set to be registered.

[0108] S40: Optimize the state variables based on the initial estimate of the minimum distance, the state variables including the translation vector and the rotation matrix;

[0109] Wherein, the minimum distance refers to a measure used in the registration process to measure the difference between two point clouds. The application uses the Euclidean distance between points. Feature matching algorithms such as SIFT, SURF, etc. can be used to estimate the initial translation and rotation, and these parameters can be optimized by minimizing the distance between feature points, or a random sample consensus algorithm can be used to estimate the initial transformation, and the parameters can be optimized by minimizing the sample consensus error. Using initial estimates for optimization can provide a good starting point in the registration process, speeding up the convergence of the optimization process. Correct initial estimates can help the algorithm avoid local optimal solutions and improve the accuracy of the registration results.

[0110] S50: finding the point cloud corresponding to each source point cloud in the second three-dimensional laser point cloud according to the nearest neighbor search, optimizing the cost function of ICP according to the SGD algorithm and the second three-dimensional laser point cloud, calculating the gradient of the optimized cost function according to the SGD-ICP algorithm, and updating the state variable corresponding to each point cloud;

[0111] In this step, the point cloud closest to the target point cloud is found by calculating the Euclidean distance between the target point cloud and other point clouds. The cost function of ICP is usually the average of the square of the Euclidean distance between each sample point in the source point cloud and the corresponding point in the target point cloud. Then a part of the point cloud samples are obtained from the second three-dimensional laser point cloud for small batch optimization to find the corresponding points of the target point cloud. By minimizing the distance between the source point cloud and the target point cloud, their relative poses can be adjusted to make them as aligned as possible. Optimizing the ICP cost function involves adjusting the relative poses between point clouds to minimize the distance between them. This process uses the SGD algorithm. Then the deviation of the position of the source point cloud after the current transformation from the corresponding point in the target point cloud is calculated, and the partial derivative with respect to translation and the deviation of the position of the source point cloud after the current transformation from the corresponding point in the target point cloud are multiplied by the partial derivative with respect to rotation. At the same time, the transformation of the source point cloud under rotation is considered, and the state variable is updated. In general, small batch optimization makes the algorithm more flexible and can handle point cloud data of various sizes and densities. Whether it is a dense point cloud or a sparse point cloud, the sample size can be adjusted to adapt. By randomly selecting sample points to calculate the gradient and update the parameters, SGD-ICP can more likely converge to the global optimal solution or a better local optimal solution. This adaptability helps SGD-ICP algorithm perform better on different datasets (i.e. point cloud data in different complex environments) and problems.

[0112] S60: generating the log gradient of the Gaussian prior of the translation, the log gradient of the von Mises prior of the rotation, and the optimal update direction of the second three-dimensional laser point cloud according to the STEIN variational gradient descent;

[0113] Specifically, for the translation of the point cloud, we can choose a Gaussian distribution as the prior probability, which can describe the translation of the point cloud in the second and third dimensions. The gradient of the log probability density function can be obtained by simply calculating the derivative of the Gaussian distribution. Therefore, for the log gradient of the Gaussian prior of translation, we can directly calculate the derivative of the log probability density function of the Gaussian distribution in the second and third dimensions. For rotation, we can choose a von Mises distribution as the prior probability, which is a probability distribution used to describe random variables in the direction of a circle. Similar to the Gaussian distribution, we can calculate the gradient of the log probability density function of the von Mises distribution, which describes the change of the rotation variable in the second and third dimensions. According to the STEIN variational gradient descent algorithm, we need to calculate the gradient of the expectation, which is the expectation of the gradient given the prior probability. For translation and rotation, we can calculate the expectation of the gradient under the corresponding prior distribution, and these expected gradients will guide us to choose the optimal update direction to better approximate the target distribution in the process of gradient descent.

[0114] S70: updating the optimal update direction of the point cloud in the second three-dimensional laser point cloud according to the gradient of the cost function, the log gradient of the Gaussian prior of translation, and the log gradient of the von Mises prior of rotation, and updating the distribution of the second three-dimensional laser point cloud using a point cloud update rule;

[0115] The gradient of the cost function is used to guide the update of the state variable, moving it towards the minimum of the cost function. During optimization, the gradient of the cost function can help move the translation variable towards the high probability region, thereby better conforming to the prior knowledge. For a given laser point cloud matching problem, the cost function usually measures the difference between two sets of point clouds, so its gradient indicates the way to move in the state space towards a better match. During optimization, the log gradient of the Gaussian prior of translation can help move the translation variable towards the high probability region, thereby better conforming to the prior knowledge. Similarly, the log gradient of the von Mises prior of rotation can guide the rotation variable to move towards the high probability region in the prior distribution, so as to better match the actual situation. When updating the optimal update direction of the point cloud, the steepest direction of the log probability and the prevention of the point cloud from collapsing into the local mode of the log probability need to be considered. The point cloud update can use a point cloud update rule to update the distribution of the entire laser point cloud, which takes into account the update of the state variable and the correspondence between the point clouds, thereby adjusting the position of the point cloud.

[0116] S80: iteratively performing steps S40-S70 until convergence, to realize the registration of the second three-dimensional laser point cloud.

[0117] The above steps employ a Transformer architecture to capture the spatial relationship between adjacent laser measurements. An anomaly detection model then refines the distribution of the normal class by clustering normal samples into a hypersphere and mapping all other anomalous samples to the center of the hypersphere. Finally, the distance from the query sample to the hypersphere center is calculated as its anomaly score. Furthermore, based on the STEIN variational inference framework, a STEIN ICP algorithm with gradient optimization is designed. This algorithm not only aligns two point clouds but also accurately estimates the uncertainty of the ICP pose alignment results, achieving superior alignment results.

[0118] Furthermore, the use of the Transformer architecture to learn the spatial relationship of the two-dimensional image includes using a local attention mechanism to associate each pixel with its neighboring area, and the attention corresponding to the pixel is shown in formula (1):

[0119]

[0120] Among them, y ij represents the weighted sum of attention at position i and position j, a and b represent the position index within the adjacent area, respectively. is the neighboring area, x ij is the feature vector representing position i and position j, q ij =W Q x ij 、k ab =W K x ab 、v ab =W V x ab , and W Q 、W K 、 They are all the corresponding first weight matrices, x ab is the eigenvector at position a and position b, that is, q ij is the query weight matrix W Q with x ij The calculated query vector, k ab is the weight matrix W of the feature vector and key at positions a and b K The calculated key vector, v ab is the weight matrix W of the eigenvectors and values ​​at positions a and b V The calculated value vector, T is the transpose of the vector, parameter r a-i,b-i is the position code to be learned, and softmax() is the calculation function of the attention weight.

[0121] Specifically, each three-dimensional laser point cloud corresponds to a two-dimensional image with approximately 57k pixels, and each pixel has 5 channels. If a global attention mechanism is used to associate each pixel with other pixels in the two-dimensional image, the computational load is very large. Therefore, the method of the present patent uses a local attention mechanism to only associate each pixel x ij with its adjacent region Since each pixel has only five values, a single-head self-attention is applied to calculate the attention of x ij as shown in equation (1). In equation (1), a weight matrix is used to reduce the number of parameters during calculation, making the model more lightweight, easier to train and deploy. By introducing position encoding and local attention mechanism, the model can flexibly process pixels at different positions and adjust the attention according to specific tasks, improving the adaptability and generalization ability of the model.

[0122] Further, the generated data set comprises:

[0123] a plurality of continuous two-dimensional laser radar images before the complex environmental disturbance are collected, and the effective measurement number of the two-dimensional laser radar images before the complex environmental disturbance is calculated as n before ;

[0124] a plurality of continuous two-dimensional laser radar images during the complex environmental disturbance are collected, and the effective measurement number of the two-dimensional laser radar images during the complex environmental disturbance is calculated as n after ;

[0125] According to the effective measurement number of the two-dimensional laser radar images before the complex environmental disturbance and the effective measurement number of the two-dimensional laser radar images during the complex environmental disturbance, the pixels are marked to generate the labeled data set and the unlabeled data set;

[0126] The labeled data set is D n , and D n = {(x1, l1), …, (x n , l n )}, where x n is the nth pixel of the two-dimensional laser radar image during the complex environmental disturbance, l n is the pixel label corresponding to the nth pixel, when n before -n after is greater than a preset value, the pixels in the two-dimensional laser radar image during the complex environmental disturbance without effective measurement are marked as l = -1, and the rest are set as l = 1;

[0127] The unlabeled data set is and where, is the pixel set with l = -1 in D n , and representing the labeled dataset D n the mth pixel with l = -1.

[0128] Specifically, taking the rainy day environment as an example, for each pixel x, if it corresponds to normal category laser measurement, it is labeled as l = 1, if it corresponds to abnormal category, it is labeled as l = -1. First, a series of laser radar scan data from sunny weather are recorded, they are converted into two-dimensional laser radar images, and all pixels corresponding to abnormal measurements are labeled as l = 1. Before the rain starts, a series of consecutive two-dimensional laser radar images are recorded in a static environment until the rain stops, ten consecutive two-dimensional laser radar images before the rain are selected, and the number of valid measurements at each location is calculated as n clear . Similarly, another ten consecutive two-dimensional laser radar images are selected from rainy days, and the number n rain is calculated. ckear If n rain is greater than a preset value, the pixels without valid measurements in the ten rainy day images are labeled as l = -1. This process simulates the data collection process in real environment. By recording laser radar scan data from sunny and rainy days, and labeling image pixels according to actual situation, a dataset closer to real situation is generated. The dataset contains data under normal and abnormal conditions, enabling the model to learn to recognize and distinguish between the two conditions, which helps to improve the robustness of the model, enabling it to make accurate predictions in various situations, and the dataset contains data acquired under different environmental conditions, which helps the model to learn to generalize to situations under various environmental conditions.

[0129] Further, the anomaly detection model is based on the labeled set for hypersphere learning, specifically:

[0130] According to the objective function of the DeepSAD model, the pixels with l = 1 in the labeled dataset are adjusted to a hypersphere, and the objective function is as shown in formula (2):

[0131]

[0132] where n is the number of pixels in the labeled dataset, m is the number of pixels in the unlabeled dataset, representing the i-th pixel with l = -1, x j representing the j-th pixel with l = 1, l jis the pixel label of the jth pixel in the unlabeled dataset, w is the weight matrix of the Transformer architecture, and is initialized with the weights of the binary classification model, parameter c is the center of the preset hypersphere in the Transformer (·; w) space, parameter η′ is a hyperparameter used to balance the weights of the labeled dataset and the unlabeled dataset, parameter λ is a hyperparameter for the regularization term, and F is the Frobenius norm.

[0133] Specifically, the anomaly detection model in this embodiment uses the DeepSAD model. The DeepSAD model provides an objective function that aims to learn a potential representation that can push normal data into a hypersphere centered on parameter c and push other abnormal data away. It includes multiple terms for fitting labeled data and unlabeled data, as well as regularizing model parameters. Such an objective function design can balance the importance of labeled and unlabeled data during training and help prevent overfitting. The center c remains unchanged during training. n During the initial pass of the normal data in , the center c is set to the mean of the Transformer(·; w). The presence of hyperparameters η′ and λ allows the model to be more flexible and adaptable to different data and tasks. By adjusting these hyperparameters, we can control the weighting of labeled and unlabeled data during training, as well as the strength of regularization.

[0134] Furthermore, the step of removing abnormal point clouds from the three-dimensional laser point cloud to obtain a second three-dimensional laser point cloud includes:

[0135] Calculate the distance d(x) from each point cloud in the three-dimensional laser point cloud to the center c of the preset hypersphere. The calculation of the distance d(x) is shown in formula (3):

[0136] d(x) = ‖Transformer(x; W * )-c‖ (3)

[0137] Where x is the point cloud in the three-dimensional laser point cloud, W * is the weight matrix of the trained Transformer architecture, and c is the center of the preset hypersphere;

[0138] The distance between each point cloud and the center c of the preset hypersphere is scaled to obtain the abnormality score d′(x) of each point cloud in the three-dimensional laser point cloud. The scaling is shown in formula (4):

[0139]

[0140] Abnormal point clouds are eliminated according to the abnormality score of each point cloud in the three-dimensional laser point cloud.

[0141] Specifically, for each point cloud x in the three-dimensional laser point cloud, it is mapped to a feature space by a Transformer architecture using the weight matrix of the trained Transformer architecture, and the distance between the point cloud to be detected and the preset hypersphere center c is calculated. The distance has no upper limit value, so the distance needs to be scaled according to formula (4), and the value range of the scaled distance is [0, 1), which is used as the abnormal score of x. According to a preset value, the point cloud with a distance greater than the preset value is removed, and a filtered three-dimensional laser point cloud, i.e., a second three-dimensional laser point cloud, is obtained.

[0142] Further, the gradient of the optimized cost function calculated according to the SGD-ICP algorithm is calculated, and the state variable corresponding to each point cloud is updated, including the steps of:

[0143] According to the cost function, the state variable before updating is partially differentiated to calculate the gradient of the translation vector and the gradient of the rotation matrix;

[0144] The state variable is updated according to the gradient of the translation vector and the gradient of the rotation matrix;

[0145] The cost function is shown in formula (5):

[0146]

[0147] Wherein, M is the number of samples obtained from the second three-dimensional laser point cloud , e is a point cloud in the sample , s e is a point cloud in the sample , r e is the point cloud with the shortest distance to s e , the state variable θ = {x, y, z, roll, pitch, yall}, wherein the first three dimensions x, y and z of θ constitute the translation amount θ 1:3 , u is the translation vector, roll is the rotation angle around the x axis, pitch is the rotation angle around the y axis, and yall is the rotation angle around the z axis, is the rotation matrix parameterized by the last three dimensions θ 4:6 of θ;

[0148] The gradient of the translation vector is as shown in formula (6):

[0149]

[0150] The gradient of the rotation matrix is as shown in formula (7):

[0151]

[0152] wherein R t is the transpose of the rotation matrix, u t is the transpose of the translation vector, is the partial derivative of the translation vector with respect to the translation amount, is the partial derivative of the rotation matrix with respect to the rotation angle, e is a sample point cloud in the sample set;

[0153] The updated state variable θ is shown in equation (8):

[0154]

[0155] wherein wherein θ t and θ t+1 are the pose parameter values of the current iteration and the next iteration, respectively, and the matrix is the preconditioner of the gradient, and the learning rate lr determines the speed of the parameter value change, is the partial derivative of the cost function with respect to the state variable θ.

[0156] Specifically, the partial derivative of θ according to equation (5) is calculated to obtain the gradient of the translation vector and the gradient of the rotation matrix. By updating the state variable through the gradient, it can move towards the local minimum of the cost function, which helps to find the optimal translation and rotation parameters to minimize the point cloud registration error. Gradient update can help the algorithm to jump out of the local optimal solution, especially in the case of poor initial point selection or multiple local optimal solutions of the cost function. The preconditioner of the gradient in equation (8) helps to improve the numerical condition of the optimization problem, making the optimization process more stable and avoiding problems caused by numerical instability, such as divergence or oscillation. Through the preconditioner matrix A, the direction and step size of parameter update can be better controlled, thereby improving the convergence of the optimization algorithm and making it easier to converge to the global optimal solution or a better solution.

[0157] Further, the generating the optimal update direction of the second three-dimensional laser point cloud comprises:

[0158] calculating the logarithmic gradient of the posterior probability based on the operator of STEIN;

[0159] deriving the optimal update direction of the point cloud according to the logarithmic gradient of the posterior probability combined with the STEIN variational gradient descent;

[0160] The operator is The operator is shown in equation (9):

[0161]

[0162] wherein, is the expectation operation, p is an intractable but differentiable posterior distribution, the function KL(·‖·) denotes the KL divergence between the point cloud distribution and the target distribution, and η is a small step size, is the derivative of the step size, q [ηφ] is specified non-parametrically by the updated point cloud x' = x + ηφ(x), where x is the un-updated point cloud, η is a small step size, φ(x) is the steepest direction, trace() is the trace operation of a matrix, and the operator The expression of the product of the steepest direction is shown in equation (10):

[0163]

[0164] According to equations (9) and (10), the logarithmic gradient of the posterior probability is calculated:

[0165]

[0166] wherein N is the total number of pixels of the data set, is the logarithmic gradient of the posterior probability, x is the point cloud before registration, D is the data set of the second three-dimensional laser point cloud, and d idx is the idxth point cloud of the second three-dimensional laser point cloud, and are the results of the derivatives of the log-likelihood function and the prior with respect to x, respectively;

[0167] The optimal update direction of the derived point cloud is φ * (·), and φ * (·) is shown in equation (12):

[0168]

[0169] wherein, is the expectation operation, k(x,·) is the kernel function, x is the point cloud before updating, is the gradient with respect to x, is the result of the derivative of the prior with respect to x.

[0170] Specifically, the operator of STEIN As shown in equation (9), when p = q, we get And combined with formula (10), the logarithmic gradient of the posterior probability can be obtained. Through the expectation operation, the contribution of all points in the point cloud distribution can be comprehensively considered, not just the contribution of a single point. This helps to globally optimize the update direction of the point cloud and improve the overall quality of the generated point cloud. KL divergence is an indicator that measures the difference between two probability distributions. When the two distributions are exactly the same, the KL divergence is zero. In this method, when the generated point cloud distribution is the same as the target distribution, that is, when p = q, the gradient of the KL divergence is zero, which means that unnecessary changes can be avoided when updating the point cloud, thereby improving efficiency. By calculating the logarithmic gradient of the posterior probability, the point cloud update direction can be determined. This gradient-based update method can more accurately guide the changes in the point cloud, making the generated point cloud closer to the target distribution.

[0171] It is worth noting that STEIN variational gradient descent is constructed by constructing a set of K particles The nonparametric variational distribution represented by is used to approximate an unsolvable but differentiable posterior distribution P(X). These particles are iteratively updated using an update rule of the following form:

[0172]

[0173] Where η is a small step size and φ describes the perturbation direction with the steepest descent direction. v is chosen to push the particle distribution closer to the target distribution. The optimal value of the perturbation direction φ can be obtained from the following expression:

[0174]

[0175] where φ * From the collection In the selection, the function KL(·‖·) represents the KL divergence between the particle distribution and the target distribution, The updated particles Specified nonparametrically.

[0176] By combining the STEIN variational gradient descent with the expression of the optimal value φ of the perturbation direction, the optimal update direction of the point cloud can be derived, which is expressed as (12)

[0177] Further explanation: In order to obtain a tractable and flexible solution, STEIN variational gradient descent chooses The unit ball on a reproducing kernel Hilbert space (RKHS) Among them consists of a scalar-valued function associated with a positive definite kernel k(x,x′), namely Formally, RKHS is a system with an inner product and norm and satisfies the regenerative property, i.e.:

[0178]

[0179]

[0180] Hilbert space of STEIN variational gradient descent is the above method, i.e. by using a set of particles to approximate the probability distribution of the point cloud, and by iteratively updating these particles to find the optimal point cloud update direction.

[0181] Further, the result of the derivative of the prior with respect to x is the logarithmic gradient of the translational Gaussian prior and the logarithmic gradient of the rotational von Mises prior, respectively as shown in equation (13) and equation (14):

[0182]

[0183]

[0184] wherein, is the logarithmic gradient of the translational Gaussian prior, is the logarithmic gradient of the rotational von Mises prior, μ 1:3 and μ 4:6 represent the translational and rotational mean values, and σ 1:3 and represent the variance of the prior distribution of the translational and rotational components, respectively.

[0185] Specifically, the logarithmic gradient of the translational Gaussian prior is calculated by the deviation of the translational component from the mean value divided by the variance, which conforms to the gradient form of the normal distribution. The logarithmic gradient of the rotational von Mises prior is calculated by the deviation of the rotational component from the mean value multiplied by a parameter that measures the rate of change, and multiplied by the derivative of the sine function to adjust the gradient. Such design can better capture the changes of the rotational component. By introducing the variance parameter and the rate parameter, the shape of the prior distribution can be flexibly controlled. The introduction of the sine function in the logarithmic gradient function of the rotation takes into account the periodicity of the rotation angle. Since the angle is usually periodic, using the sine function can better handle this periodicity, ensuring that the gradient does not jump when the angle changes to the boundary, thus making the optimization process smoother and more stable.

[0186] Further, the point cloud update rule is shown in equation (15):

[0187]

[0188] wherein, x is the j'th point cloud before registration, x j′ is the x'th point cloud after registrationj′ a point cloud, η is a small step size, is the steepest direction, as shown in equation (16):

[0189]

[0190] wherein K is the total number of point clouds of the second three-dimensional laser point cloud, is weighted and smoothed by a kernel function, and is the steepest direction of the log probability, is the gradient of the kernel function, x is the j'th point cloud before registration, and x j′ is the x'th point cloud after registration; j′ .

[0191] The updating of the optimal update direction of the point cloud in the second three-dimensional laser point cloud according to the gradient of the cost function, the log gradient of the Gaussian prior of the translation, and the log gradient of the von Mises prior of the rotation, and the updating of the distribution of the second three-dimensional laser point cloud using the point cloud update rule include:

[0192] The gradient of the translation vector and the log gradient of the Gaussian prior of the translation are substituted into to obtain the updated translation vector of the point cloud;

[0193] The gradient of the rotation matrix and the log gradient of the von Mises prior of the rotation are substituted into to obtain the updated rotation matrix of the point cloud;

[0194] The optimal update direction of each point cloud is calculated according to the gradient of the cost function, the updated translation vector, and the updated rotation matrix, and the steepest direction in equation (15) is substituted to update the distribution of the point cloud;

[0195] The optimal update direction of each point cloud is as shown in equation (17):

[0196]

[0197] wherein μ is the mean value, K is the total number of point clouds of the second three-dimensional laser point cloud, θ j′ is the state variable of the j'th point cloud, is the difference between μ and θ j , is the log gradient of the state variable, is the optimal update direction of each point cloud, is the prior gradient of the state variable, k(θ j′ , θ) is a kernel function, is the gradient of the kernel function with respect to θ.

[0198] Specifically, in the STEIN variational gradient descent combined with the ICP algorithm, the gradient of the cost function and the prior gradient of translation and rotation are substituted into formula (16), the first term of formula (16) is weighted by a kernel function, smoothing the gradient and serving as the steepest direction of the log probability, and the second term is the gradient of the kernel function, which can be regarded as a repulsive force to promote the diffusion of particles and prevent them from collapsing into local modes of the log probability. Through the above steps, the updates of translation (θ 1:3 ) and rotation (θ 4:6 ) in formula (17) for all particles can be obtained independently, and the optimal update direction of each point in the point cloud is updated, that is, the translation and rotation of the point cloud are calculated using and and respectively, and are applied to formula (17), the first term includes the negative value of the average of the state variable θ j and the negative gradient of the log prior probability, which can be understood as a penalty term for the gradient, helping to guide the update direction to a more likely state. The kernel function k(θ j, ,θ) measures the similarity between the state variable θ j and the update position θ. The kernel function is commonly used in machine learning to measure the similarity between samples, which can also be understood as measuring the similarity between the current position and the particle position. And substituting formula (17) into the steepest direction of formula (15), that is, the registered point cloud is obtained, that is. Combined with the STEIN variational gradient descent and the ICP algorithm, by using the gradient of the cost function and the prior information, as well as the calculated optimal update direction, the point cloud registration can be more effectively performed. This can make the registration process faster and more accurate, based on the variational gradient descent and the ICP algorithm, and combined with the use of prior probability and penalty term, it has good theoretical basis support, which helps to understand the registration process and further optimize and improve.

[0199] Further, the kernel function includes a kernel function corresponding to translation and a kernel function corresponding to rotation.

[0200] The kernel function corresponding to translation is shown in formula (18):

[0201]

[0202] The kernel function corresponding to rotation is shown in formula (19):

[0203]

[0204] where θ 1:3where x, y and z are the translation amounts in the x, y and z dimensions, respectively, and θ' is the solution target 1:3 where x, y and z are the translation amounts in the x, y and z dimensions, respectively, and θ' is the solution target 1:3 where x, y and z are the translation amounts in the x, y and z dimensions, respectively, and θ' is the solution target 4:6 where h is the bandwidth of the kernel function, k() represents the kernel function, and θ' is the solution target is the square of the two-norm of the vector, arctan2() is a variant of the arctangent function, and sin(θ 4:6 - θ' ′ 4:6 is the rotation sine value of the point cloud, cos(θ 4:6 - θ' 4:6 is the rotation cosine value of the point cloud, and exp() represents the natural exponential function.

[0205] Specifically, in the STEIN variational gradient descent, the kernel function plays a crucial role in the weighted gradient and the scattered particles, where h is the bandwidth of the kernel function, which controls the decay rate of the kernel function and affects the width and shape of the kernel function. A larger h value will result in a wider shape of the kernel function, making it easier to accept the similarity between adjacent points, while a smaller h value will result in a sharper kernel function, only very close points will be considered similar; θ' is the solution target.

[0206] It is worth noting that the Euclidean distance is not directly applicable to angles because angles are periodic. Therefore, when calculating the similarity between rotation parameters, some specific angle difference measures are often used, so the kernel function corresponding to the rotation is set as shown in equation (19). Such a measure not only considers the difference between angles, but also considers its periodicity, so it is more suitable for use as a distance measure in the kernel function.

[0207] In the description of the present specification, the description of the terms "one embodiment", "some embodiments", "exemplary embodiment", "example", "specific example", or "some examples" means that the specific features, structures, materials or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present application. In the present specification, the exemplary description of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.

[0208] Although embodiments of the present application have been shown and described, those skilled in the art can understand that various changes, modifications, replacements and variations can be made to these embodiments without departing from the principles and spirit of the present application, and the scope of the present application is defined by the claims and their equivalents.

Claims

1. A method for unmanned boat lidar point cloud registration facing complex environmental interference, characterized by: Including steps: S10: converting the three-dimensional laser point cloud to be registered into a two-dimensional image, learning the spatial relationship of the two-dimensional image using a Transformer architecture, and creating a binary classification model based on the Transformer architecture; S20: Generate a data set, and perform binary classification on the three-dimensional laser point cloud according to the binary classification model and the data set, wherein the data set includes a labeled data set and an unlabeled data set; S30: Based on the anomaly detection model, using the data set to perform hypersphere learning, remove abnormal point clouds from the three-dimensional laser point cloud, and obtain a second three-dimensional laser point cloud; S40: Optimizing state variables based on an initial estimate of the minimum distance, the state variables including a translation vector and a rotation matrix; S50: searching for a point cloud corresponding to each source point cloud in the second three-dimensional laser point cloud according to a nearest neighbor search, optimizing an ICP cost function according to an SGD algorithm and the second three-dimensional laser point cloud, calculating a gradient of the optimized cost function according to an SGD-ICP algorithm, and updating the state variable corresponding to each point cloud; S60: Generate, according to STEIN variational gradient descent, a logarithmic gradient of a Gaussian prior of a translation, a logarithmic gradient of a von Mises prior of a rotation, and an optimal update direction of the second three-dimensional laser point cloud; S70: updating the optimal update direction of the point cloud in the second three-dimensional laser point cloud according to the gradient of the cost function, the logarithmic gradient of the Gaussian prior of the translation, and the logarithmic gradient of the vonMises prior of the rotation, and updating the distribution of the second three-dimensional laser point cloud using a point cloud update rule; S80: Iterate steps S40-S70 until convergence, and realize registration of the second three-dimensional laser point cloud for accurately estimating the position and attitude of the unmanned boat.

2. The point cloud registration method according to claim 1, characterized in that: The use of the Transformer architecture to learn the spatial relationship of the two-dimensional image includes using a local attention mechanism to associate each pixel with its neighboring area. The attention corresponding to the pixel is shown in formula (1): Among them, y ij represents the weighted sum of attention at position i and position j, a and b represent the position index within the adjacent area, respectively. is the neighboring area, x ij is the feature vector representing position i and position j, q ij =W Q x ij 、k ab =W K x ab 、v ab =W v x ab , and W Q 、W K 、 They are all the corresponding first weight matrices, x ab is the eigenvector at position a and position b, that is, q ij is the query weight matrix W Q with x ij The calculated query vector, k ab is the weight matrix W of the feature vector and key at positions a and b K The calculated key vector, v ab is the weight matrix W of the eigenvectors and values ​​at positions a and b V The calculated value vector, T is the transpose of the vector, parameter r a-i,b-i is the position code to be learned, and softmax() is the calculation function of the attention weight.

3. The point cloud registration method according to claim 2, characterized in that: The generating data set comprises: Collect several continuous two-dimensional lidar images before the complex environment interference, and calculate the effective measurement number of the two-dimensional lidar images before the complex environment interference as n before ; Collect several continuous two-dimensional lidar images under complex environmental interference and calculate the effective measurement times n of the two-dimensional lidar images under complex environmental interference after ; Marking pixels according to the effective measurement times of the two-dimensional lidar image before the complex environment interference and the effective measurement times of the two-dimensional lidar image during the complex environment interference, to generate the labeled data set and the unlabeled data set; The labeled dataset is D n , and D n ={(x1,l1),…,(x n ,l n )}, where x n is the nth pixel of the 2D lidar image under complex environment interference, l n is the pixel label corresponding to the nth pixel, when n before -n after If it is greater than a preset value, the pixels without valid measurements in the two-dimensional lidar image under complex environment interference are marked as l = -1, and the rest are set to l = 1; The unlabeled dataset is and in, D n The set of pixels where l = -1, Represents the labeled dataset D n The mth pixel where l=-1.

4. The point cloud registration method according to claim 3, characterized in that: The method of performing hypersphere learning using the data set based on the anomaly detection model is as follows: The pixels with l=1 in the labeled dataset are adjusted to a hypersphere according to the objective function of the DeepSAD model, and the objective function is shown in formula (2): Where n is the number of pixels in the labeled dataset, m is the number of pixels in the unlabeled dataset, represents the i-th pixel with l=-1, x j represents the jth pixel with l=1, l j is the pixel label of the j-th pixel in the unlabeled dataset, is the weight matrix of the Transformer architecture and is initialized using the weights of the binary classification model. The parameter c is in The center of the preset hypersphere in space, the parameter η′ is a hyperparameter used to balance the weights of the labeled dataset and the unlabeled dataset, the parameter λ is a hyperparameter for the regularization term, and F is the Frobenius norm.

5. The point cloud registration method according to claim 4, characterized in that: Eliminating abnormal point clouds from the three-dimensional laser point cloud to obtain a second three-dimensional laser point cloud includes: Calculate the distance d(x) from each point cloud in the three-dimensional laser point cloud to the center c of the preset hypersphere. The calculation of the distance d(x) is shown in formula (3): d(x)=‖Transformer(x;W * )-c‖ (3) Where x is the point cloud in the three-dimensional laser point cloud, W * is the weight matrix of the trained Transformer architecture, and c is the center of the preset hypersphere; The distance between each point cloud and the center c of the preset hypersphere is scaled to obtain the abnormality score d′(x) of each point cloud in the three-dimensional laser point cloud. The scaling is shown in formula (4): Abnormal point clouds are eliminated according to the abnormality score of each point cloud in the three-dimensional laser point cloud.

6. The point cloud registration method according to claim 1, characterized in that: The step of calculating the gradient of the optimized cost function according to the SGD-ICP algorithm and updating the state variables corresponding to each point cloud includes the following steps: Performing partial derivatives on the state variables before updating according to the cost function to calculate the gradient of the translation vector and the gradient of the rotation matrix; updating the state variable according to the gradient of the translation vector and the gradient of the rotation matrix; The cost function is shown in formula (5): Where M is the sample obtained from the second three-dimensional laser point cloud The number of samples, e is A point cloud in , s e For samples A point cloud in r e For s e The point cloud with the shortest distance has a state variable θ = {x, y, z, roll, pitch, yall}, where the first three dimensions of θ, x, y, and z, constitute the translation θ. 1:3 , u is the translation vector, roll is the rotation angle around the x-axis, pitch is the rotation angle around the y-axis, yall is the rotation angle around the z-axis, is the third dimension of θ 4:6 parameterized rotation matrices; The gradient of the translation vector is As shown in formula (6): The gradient of the rotation matrix is As shown in formula (7): Among them, R t is the transpose of the rotation matrix, u t is the transpose of the translation vector, is the partial derivative of the translation vector and the translation amount, is the partial derivative of the rotation matrix with respect to the rotation angle, and e is the sample A point cloud in The updated state variable θ is shown in formula (8): Among them, θ t and θ t+1 are the posture parameter values ​​of the current iteration and the next iteration, respectively, and the matrix is a preconditioner of the gradient, and the learning rate lr determines how fast the parameter value changes. is the partial derivative of the cost function with respect to the state variable θ.

7. The point cloud registration method according to claim 1, characterized in that: The optimal update direction for generating the second three-dimensional laser point cloud includes: Based on the STEIN operator, the logarithmic gradient of the posterior probability is calculated; According to the logarithmic gradient of the posterior probability, the optimal update direction of the point cloud is derived in combination with STEIN variational gradient descent; The operator is The operator is shown in formula (9): in, To obtain the expected operation, p is an unsolvable but differentiable posterior distribution, the function KL(·‖·) represents the KL divergence between the point cloud distribution and the target distribution, and η is a small step size. is the derivative of the step size, q [ηφ] It is specified nonparametrically by the updated point cloud x′=x+ηφ(x), where x is the unupdated point cloud, η is a small step size, φ(x) is the steepest direction, trace() is the matrix trace operation, and the operator The expression of the product of the steepest direction is shown in formula (10): According to equations (9) and (10), the logarithmic gradient of the posterior probability is calculated: Where N is the total number of pixels in the dataset, is the logarithmic gradient of the posterior probability, x is the point cloud before registration, D is the data set of the second three-dimensional laser point cloud, d idx is the idx-th point cloud of the second three-dimensional laser point cloud, and are the log-likelihood function and the prior derivative of x; The optimal update direction of the exported point cloud is φ * (·),φ * (·) As shown in formula (12): in, is the expected operation, k(x,·) is the kernel function, x is the point cloud before update, is the gradient with respect to x, is the result of the prior derivative with respect to x.

8. The point cloud registration method according to claim 7, characterized in that: The result of the prior derivative with respect to x is the logarithmic gradient of the Gaussian prior for the translation and the logarithmic gradient of the von Mises prior for the rotation, as shown in Equations (13) and (14), respectively: in, is the log gradient of the translated Gaussian prior, is the logarithmic gradient of the rotated vonMises prior, μ 1:3 and μ 4:6 represents the translation and rotation mean, and σ 1:3 and Represent the variance of the prior distribution of the translation and rotation components respectively.

9. The point cloud registration method according to claim 6, characterized in that: The point cloud update rule is shown in formula (15): Among them, x is the point cloud before the j′th registration, x j′ is the j′th point cloud after registration, η is a small step size, is the steepest direction, As shown in formula (16): Wherein, K is the total amount of point clouds of the second three-dimensional laser point cloud, To perform weighted processing through the kernel function, smooth the gradient and use it as the steepest direction of the logarithmic probability, is the gradient of the kernel function, x is the jth ′ Point cloud before registration, x j′ is the xth j′ point clouds; Updating the optimal update direction of the point cloud in the second three-dimensional laser point cloud according to the gradient of the cost function, the logarithmic gradient of the Gaussian prior of the translation, and the logarithmic gradient of the vonMises prior of the rotation, and updating the distribution of the second three-dimensional laser point cloud using a point cloud update rule includes: Substitute the gradient of the translation vector and the logarithmic gradient of the Gaussian prior of the translation into , obtaining the updated translation vector of the point cloud; Substitute the gradient of the rotation matrix and the logarithmic gradient of the rotation von Mises prior into , obtaining the updated rotation matrix of the point cloud; The optimal update direction of each point cloud is calculated according to the gradient of the cost function, the updated translation vector and the updated rotation matrix, and the steepest direction in formula (15) is substituted to update the point cloud distribution; The optimal update direction of each point cloud is shown in formula (17): Where μ is the average value, K is the total number of point clouds of the second three-dimensional laser point cloud, θ j′ is the state variable of the j′th point cloud, μ and θ j The difference, is the logarithmic gradient of the state variable, is the optimal update direction for each point cloud, is the prior gradient of the state variable, k(θ j′ ,θ) is the kernel function, is the gradient of the kernel function with respect to θ.

10. The point cloud registration method according to claim 9, characterized in that: The kernel function includes a kernel function corresponding to translation and a kernel function corresponding to rotation; The kernel function corresponding to the translation is shown in formula (18): The kernel function corresponding to the rotation is shown in formula (19): Among them, θ 1:3 The first three dimensions x, y and z of θ constitute the translation, θ′ 1:3 For θ 1:3 The solution goal, θ 4:6 are the rotation angles for the x, y and z axes respectively, h is the bandwidth of the kernel function, is the square of the vector's two-norm, arctan2() is a variant of the inverse tangent function, sin(θ 4:6 -θ′ 4:6 ) is the rotation sine value of the point cloud, cos(θ 4:6 -θ′ 4:6 ) is the rotation cosine value of the point cloud, exp() represents the natural exponential function, and k() represents the kernel function.

Images