Laser SLAM point cloud registration method based on locatability detection

Abstract

The invention discloses a laser SLAM point cloud registration method based on localization detection, and belongs to the technical field of point cloud data processing, and the method comprises the steps: obtaining a to-be-registered radar point cloud, and extracting plane feature points in the to-be-registered radar point cloud; establishing a point-to-plane ICP cost function; constructing an information matrix of the current point cloud by using a cost function, and decomposing the information matrix to obtain a matrix only containing rotation and translation information, and respective eigenvalues and eigenvectors; obtaining rotation information and translation information pairs of each plane feature point to obtain an information vector of the current point cloud; calculating the localizability of each point, and screening plane feature points; and reconstructing a cost function from the point to the plane ICP by using the screened plane feature points, and performing optimization to obtain the optimal pose transformation of the current point cloud so as to realize point cloud registration. According to the method, the problem of point cloud registration deterioration caused by the SLAM algorithm in a feature degradation environment can be solved.

Description

Technical Field

[0001] The invention relates to the technical field of point cloud data processing, and in particular to a laser SLAM point cloud registration method based on locatability detection. Background Art

[0002] As artificial intelligence is booming, the field of robotics is also developing rapidly. Various robots have entered people's lives and are playing an increasingly important role. In industrial production, mobile robots can replace humans to complete many complex, dangerous, and high-precision tasks, and are not affected by fatigue, emotions, or other human factors, greatly improving work efficiency and costs.

[0003] If robots want to achieve autonomous movement, they must rely on positioning and mapping. Outdoors, we can use high-precision, wide-coverage satellite positioning and existing high-precision maps to enable robots to obtain location information in real time and plan paths, thereby achieving autonomous movement. However, indoors, where there are no satellite positioning signals and prior environmental maps, mobile robots must solve the problem of simultaneous positioning and environmental map construction (SLAM). SLAM technology uses motion sensing sensors such as lidar, depth cameras, and IMU (inertial measurement units) to enable robots to locate their own positions in unknown environments while building environmental maps. Through SLAM, robots can autonomously navigate in dynamic environments, avoid obstacles, and update environmental information in real time, which provides important support for them to perform tasks in complex indoor environments. SLAM technology is not only used for indoor robot navigation. With the development of technology, its application has gradually expanded to multiple fields such as autonomous driving, augmented reality (AR), and embodied intelligence.

[0004] Traditional LiDAR ranging mainly uses geometric measurement methods, ignoring texture and color information. This method is unreliable in environments with scarce and repeated features, such as tunnels and long corridors. The reduction in the number of feature points will lead to ambiguity in scan matching, resulting in reduced accuracy of robot pose estimation. In geometric self-symmetric environments, since the geometric constraints along the symmetry axis are difficult to distinguish from noise, the point cloud registration algorithm converges to a non-optimal solution caused by noise, and even the optimization fails to converge due to the reduction in the number of features that constitute the constraints, which will reduce the accuracy of robot pose estimation. The environment that causes this result is a degraded environment. At present, the main method to solve the problem of point cloud registration deterioration in degraded environments is to evaluate the effect of pose estimation through the relationship between the point cloud registration cost function and the geometric features of the environment. This method only considers the influence of geometric features and ignores the influence of global errors and local noise, resulting in poor pose estimation in degraded environments. Summary of the invention

[0005] The invention provides a laser SLAM point cloud registration method based on locatability detection, so as to solve the technical problem that the existing laser SLAM point cloud registration method has low accuracy.

[0006] In order to solve the above technical problems, the present invention provides the following technical solutions:

[0007] On the one hand, the present invention provides a laser SLAM point cloud registration method based on locatability detection, and the laser SLAM point cloud registration method based on locatability detection includes:

[0008] Obtain the radar point cloud to be registered and extract the plane feature points therein;

[0009] Based on the extracted plane feature points, a cost function of point-to-plane ICP is established;

[0010] The information matrix of the current point cloud is constructed using the cost function, and the information matrix is ​​decomposed to obtain a matrix containing only rotation information and a matrix containing only translation information, as well as the eigenvalues ​​and eigenvectors of the rotation information and translation information;

[0011] Obtain the rotation information and translation information pair of each plane feature point to obtain the information vector of the current point cloud;

[0012] Based on the information vector, calculate the locatability contribution value of each point and screen the plane feature points;

[0013] The screened plane feature points are used to reconstruct the cost function of point-to-plane ICP, and the reconstructed cost function is optimized. The optimal pose transformation of the current point cloud is obtained through iterative solution to achieve point cloud registration.

[0014] Furthermore, the step of obtaining the radar point cloud to be registered and extracting the plane feature points therein includes:

[0015] Obtain the radar point cloud to be registered, remove the five points at the front and back ends of each scanning line in the point cloud, and then calculate the curvature value of each remaining point;

[0016] Points whose curvature values ​​are less than a first preset threshold are extracted as plane feature points.

[0017] Furthermore, based on the extracted plane feature points, a cost function of point-to-plane ICP is established, including:

[0018] Transform the plane feature points from the radar coordinate system to the world coordinate system;

[0019] After completing the coordinate system conversion, for each plane feature point, query the 5 feature points closest to it, construct an overdetermined equation, and calculate the plane feature corresponding to the current point through QR decomposition;

[0020] Based on the plane features corresponding to each point, a cost function of point-to-plane ICP is established.

[0021] Furthermore, the cost function is used to construct the information matrix of the current point cloud, decompose the information matrix, obtain a matrix containing only rotation information and a matrix containing only translation information, and eigenvalues ​​and eigenvectors of the rotation information and translation information, including:

[0022] Linearize the rotation matrix in the cost function, the formula is:

[0023]

[0024] Where R represents the linearized rotation matrix; α represents the angle of rotation around the x-axis; β represents the angle of rotation around the y-axis; γ represents the angle of rotation around the z-axis; |r| × represents the antisymmetric matrix of the rotation vector r = (α, β, γ); I represents the identity matrix;

[0025] Substituting the linearized rotation matrix into the expression of the cost function, we get:

[0026]

[0027] Among them, ε represents the cost function; Represents the position and posture of the i-th plane feature point in the world coordinate system at time k; represents the pose of the point on the feature plane corresponding to the i-th plane feature point in the world coordinate system at time k; t represents the translation vector of the pose transformation of the current point cloud from the radar coordinate system to the world coordinate system; n k,i represents the normal vector of the i-th feature plane at time k; N represents the number of feature points in the point cloud of the current frame; r represents the rotation vector;

[0028] Convert the cost function solution process into a quadratic optimization problem:

[0029]

[0030] Among them, x k,i is the variable to be optimized; T represents the transpose of the matrix; A represents the Jacobian matrix of the optimization problem; b′ represents the constraint relationship between the point clouds; Const is a constant;

[0031] The Hessian matrix H is approximated by the Jacobian matrix of the optimization problem;

[0032] Performing principal component analysis on H, we can obtain a matrix containing only rotation information and a matrix containing only translation information, as well as the eigenvalues ​​and eigenvectors of the rotation information and translation information.

[0033] Furthermore, the step of obtaining a pair of rotation information and translation information of each plane feature point to obtain an information vector of the current point cloud includes:

[0034] Construct an information vector for each plane feature point, the formula is as follows:

[0035]

[0036] Among them, d k,i represents the information vector of the i-th plane feature point at time k, where i = 1, 2, ..., N, and N represents the number of feature points in the current frame point cloud; represents the position and posture of the i-th plane feature point in the world coordinate system at time k; n k,i represents the normal vector of the i-th feature plane at time k; ‖‖2 represents the bi-norm of the orientation quantity.

[0037] Furthermore, based on the information vector, the locatability contribution value of each point is calculated, and the plane feature points are screened, including:

[0038] Merge the information vectors of all points into matrices containing rotation information respectively and a matrix containing translation information The formula is:

[0039]

[0040] Where T represents the transpose of the matrix; N represents the number of feature points in the point cloud of the current frame;

[0041] Calculate the localizability contribution matrix of each information pair in the characteristic direction of the rotation matrix and the translation information matrix respectively and Combined with L r and L t , and obtain the localizability contribution matrix containing all dimensional information The formula is:

[0042]

[0043]

[0044]

[0045] Among them, |·| means taking the absolute value of each element in the vector; V r The eigenvector matrix representing the rotation information; Vt The eigenvector matrix representing the translation information; each row vector in L represents the projection of each information vector on the eigenvector, and each element represents the localizability contribution value of the corresponding plane feature point in each degree of freedom of posture;

[0046] Filter out the elements in L that are smaller than a second preset threshold value to obtain a filtered localization contribution matrix;

[0047] The filtered localizability contribution matrix is ​​summarized into the localizability contribution I(j) according to each posture degree of freedom:

[0048]

[0049] Wherein, L′(i,j) represents the element value of the i-th row and j-th column in the filtered localization contribution matrix;

[0050] It is determined whether the locatability contribution corresponding to each degree of freedom is greater than a third preset threshold value. If it is greater than the third preset threshold value, all plane points in the locatability contribution matrix constituting the current degree of freedom are screened out.

[0051] Furthermore, the third preset threshold is determined according to the laser radar used.

[0052] Furthermore, the optimizing the reconstruction cost function includes:

[0053] The Gauss-Newton iteration method is used to optimize the reconstruction cost function.

[0054] On the other hand, the present invention further provides an electronic device, comprising a processor and a memory; wherein the memory stores at least one instruction, and the instruction is loaded and executed by the processor to implement the above method.

[0055] In yet another aspect, the present invention further provides a computer-readable storage medium, wherein at least one instruction is stored in the storage medium, and the instruction is loaded and executed by a processor to implement the above method.

[0056] The beneficial effects brought about by the technical solution provided by the present invention include at least:

[0057] The present invention uses the feature information of the optimization space to filter out points with large locatability contributions, and uses these points to reconstruct the ICP cost function to obtain the current pose transformation, thereby making full use of the locatability state of the main direction of the point cloud registration optimization problem, and uses the screened points with large locatability contributions to reconstruct the ICP cost function and iteratively solve it, discarding unreliable points so that the cost function can converge faster and obtain the optimal pose transformation. The problem of the current laser SLAM algorithm causing point cloud registration to deteriorate in a feature-degraded environment is solved. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0059] Figure 1 It is a schematic diagram of the execution flow of the laser SLAM point cloud registration method based on locatability detection provided by an embodiment of the present invention;

[0060] Figure 2 is an example of the information vector of a point constructed in an embodiment of the present invention in a two-dimensional space;

[0061] Figure 3 It is a system block diagram of an electronic device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0062] In order to make the objectives, technical solutions and advantages of the present invention more clear, the embodiments of the present invention will be further described in detail below with reference to the accompanying drawings.

[0063] First of all, it should be noted that in the embodiments of the present invention, words such as "exemplarily" and "for example" are used to indicate examples, illustrations or explanations. Any embodiment or design described as "exemplary" in the present invention should not be interpreted as being more preferred or more advantageous than other embodiments or designs. Specifically, the use of the word "exemplarily" is intended to present the concept in a concrete way. In addition, in the embodiments of the present invention, the meaning expressed by "and / or" can be both, or it can be either of the two.

[0064] First embodiment

[0065] This embodiment provides a laser SLAM point cloud registration method based on locatability detection, which uses feature information in the optimization process and plane feature information in the environment to establish a measurement index, and uses a threshold to determine whether degradation occurs, and the selected reliable constraints are used to reconstruct the optimized cost function to solve the optimal posture; the method can be implemented by an electronic device, and the execution process of the method is as follows Figure 1 As shown, the following steps are included:

[0066] S1, obtain the radar point cloud to be registered and extract the plane feature points therein;

[0067] Specifically, in this embodiment, the above S1 includes:

[0068] S11, remove five points at the front and back ends of each scan line in the point cloud;

[0069] S12, calculate the curvature value of each remaining point, as shown in formula (1):

[0070]

[0071] Where c represents the curvature value; represents the depth value of the i-th point in the k-th scan in the radar coordinate system. In this embodiment, the 10 neighboring points of the point are taken to calculate its curvature. Indicates the depth value of the neighboring point of the i-th point in the k-th scan in the radar coordinate system.

[0072] S13, sorting according to the curvature size, and extracting points with curvature less than a threshold as plane feature points.

[0073] S2, based on the extracted plane feature points, establish the cost function of point-to-plane ICP;

[0074] Specifically, in this embodiment, the above S2 includes:

[0075] S21, transform the plane feature points in the source point cloud from the radar coordinate system to the world coordinate system: as shown in formula (2):

[0076]

[0077] Among them, x k,i =(r x ,r y ,r z ,t x ,t y ,t z ) is the transformation vector of the radar coordinate system relative to the world coordinate system at time k, is the position information of the i-th feature point in the radar coordinate system at time k, is the pose information of the i-th feature point in the world coordinate system at time k.

[0078] S22, build matching pairs of point clouds: query and point The overdetermined equation is constructed with the five nearest feature points, and the plane feature corresponding to the point is calculated by QR decomposition, as shown in formula (3):

[0079]

[0080] Among them, x i ,y i 、z i is the nearest neighbor of the point to be matched, A, B, and C are the plane coefficients of the feature plane corresponding to the current point to be matched.

[0081] S23, using point-to-plane matching pairs to construct the cost function of point-to-plane ICP, the entire equation consists of all plane feature points in the current point cloud and their corresponding feature planes. Minimize the cost function to find an optimal pose transformation so that the feature points in the radar coordinate system After conversion to the world coordinate system, The distance to the corresponding feature plane is the smallest, as shown in formula (4):

[0082]

[0083] in, They are the i-th plane feature point in the world coordinate system at time k and the point on its corresponding feature plane, n k,i is the normal vector of the i-th feature plane at time k, R and t are the rotation matrix and translation vector of the pose transformation of the current point cloud from the radar coordinate system to the world coordinate system.

[0084] S3, constructing the information matrix of the current point cloud using the cost function, decomposing the information matrix, obtaining a matrix containing only rotation information and a matrix containing only translation information, as well as eigenvalues ​​and eigenvectors of the rotation information and translation information;

[0085] Specifically, in this embodiment, the above S3 includes:

[0086] S31, linearized rotation matrix R; wherein the rotation matrix R can be linearized by small angle approximation, and linearized according to the angles α, β, and γ of rotation around the x, y, and z axes, as shown in formula (5):

[0087]

[0088] Substituting into formula (4) we get:

[0089]

[0090] S32, in order to analyze the localizability information in the current point cloud registration problem, the solution process of the above cost function can be converted into a quadratic optimization problem:

[0091]

[0092] in, is the variable to be optimized, and is represented by the Lie algebra and translation vector, The Jacobian matrix representing the optimization problem, represents the Hessian matrix approximated by the Jacobian matrix of the optimization problem, The constraint relationship between point clouds is integrated.

[0093] S33, perform principal component analysis on H to obtain A′ rr , A′ rt , A′ tr , Four information matrices, take out the matrix A′ containing only rotation and translation information rr and A′ tt , and their respective eigenvalues ​​and eigenvectors:

[0094]

[0095] in, is the eigenvector matrix of the rotation information, Σ r is the eigenvalue matrix of the rotation information; is the eigenvector matrix of translation information, Σ t is the eigenvalue matrix of the translation information.

[0096] S4, obtaining the rotation information and translation information pair of each plane feature point to obtain the information vector of the current point cloud;

[0097] Specifically, in this embodiment, the above S4 implementation process is as follows:

[0098] Construct an information vector for each point in the current point cloud, as shown in formula (9), and merge the information vectors of all points into matrices containing rotation information and a matrix containing translation information As shown in formula (10):

[0099]

[0100]

[0101] A two-dimensional example of the information vector for each point is Figure 2 shown.

[0102] in, Represents the cross product of the current point and the corresponding plane normal vector. You can refer to the "wrench system" from the origin of the coordinate system to point p i The vector is the applied force, point The normal vector n of the corresponding plane k,i The result of the cross product is the torque. According to the physical meaning, it can be considered that the torque generated by each point with its corresponding normal vector as the axis is the direction of the rotational locatability contribution. The translational locatability contribution is composed of the normal vector of the characteristic plane corresponding to each point. It can be understood that the locatability contribution direction generated by each point is the direction of the normal vector of its corresponding characteristic plane.

[0103] S5, based on the information vector, calculate the locatability contribution value of each point and screen the plane feature points;

[0104] Specifically, in this embodiment, the above S5 calculates and summarizes the localizability contribution value of the information vector, thereby screening out plane points with larger localizability contribution value, which specifically includes:

[0105] S51, calculate the upper localizability contribution matrix of each information pair in the characteristic direction of the rotation and translation information matrix respectively And combine to get the localizability contribution matrix containing all dimensional information Each row vector represents the projection of each information vector on the feature vector, and each element represents the localizability contribution value of each posture degree of freedom of the plane point, as shown in equations (11), (12), and (13):

[0106]

[0107]

[0108] The operator |·| means that each element in the vector takes the absolute value.

[0109] S52, filtering the locatability contribution vector composed of all feature points of the current frame, and filtering out the elements whose contribution value is less than the threshold:

[0110]

[0111] Among them, the contribution threshold limits the size of the locatability contribution value of each point in the current pose feature direction.

[0112] S53, the filtered localization contribution matrix is ​​summarized into the localizability contribution I(j) according to each posture, and it is determined whether I(j) is greater than the threshold k:

[0113]

[0114] S54, based on I(j), determine the locatability of the current posture, and determine whether each degree of freedom is greater than a given threshold k, then it is considered to be locatable at the degree of freedom, so all the plane points in the locatability contribution matrix of the degree of freedom are screened out. Among them, the threshold k is usually determined by the laser radar used. For example, this embodiment uses a 16-line Velodyne laser radar, and its one-frame point cloud has a total of 16*1800 scanning points, so k is set to 180. Among them, the larger the k, the more it can increase the accuracy of environmental degradation detection, but it will increase the computational cost; the smaller the k, the more it can reduce the computational cost, but it will reduce the accuracy of locatability detection.

[0115] S6, using the screened plane feature points to reconstruct the cost function of the point to plane ICP, and optimize the reconstructed cost function, and obtain the optimal pose transformation of the current point cloud through iterative solution to achieve point cloud registration.

[0116] Specifically, in this embodiment, the above S6 uses all the points screened in S5 to reconstruct the point-to-plane ICP residual equation, and uses the Gauss-Newton iteration method to optimize the residual equation. The iteration method is iterated as follows:

[0117]

[0118] J T J·△x=-J T ε re (17)

[0119] In the formula, J T J is the Jacobian matrix of the residual equation relative to the optimization variable, ε re is the residual equation result, △x is the optimization variable increment.

[0120] According to the definition of the residual equation below, the Jacobian matrix J can be obtained by the chain rule, and the calculation formula is:

[0121]

[0122] Divide the Jacobian matrix J in equation (18) into two parts J = [J r J t ], respectively, the rotation part of the residual equation relative to the optimization variable r = [r x ,r y ,r z ] and the translation part t=[t x ,t y ,t z ], so J can be decomposed into:

[0123]

[0124]

[0125] Among them, D(·) calculates the distance from the point to the corresponding feature plane, and G(·) transforms the point in the radar coordinate system to the world coordinate system.

[0126] In summary, this embodiment provides a laser SLAM point cloud registration method based on locatability detection, which uses the feature information of the optimization space to screen out points with large locatability contributions, and uses these points to reconstruct the ICP cost function to obtain the current pose transformation, thereby making full use of the locatability state of the main direction of the point cloud registration optimization problem, and uses the screened out points with large locatability contributions to reconstruct the ICP cost function and iteratively solve it, discarding unreliable points so that the cost function can converge faster and obtain the optimal pose transformation. This solves the problem that the current laser SLAM algorithm causes point cloud registration to deteriorate in a feature-degraded environment.

[0127] Second embodiment

[0128] This embodiment provides an electronic device, such as Figure 3 As shown, the electronic device includes: a processor and a memory; wherein the processor and the memory can be connected via a communication bus; the memory stores at least one instruction, and the instruction is loaded and executed by the processor to implement the method of the first embodiment. In addition, the electronic device may also include a transceiver, the processor and the transceiver can be connected via a communication bus, and the transceiver is used to communicate with other devices.

[0129] Next, combine Figure 3 The following is a detailed introduction to the various components of the electronic device:

[0130] Among them, the processor is the control center of the electronic device, and the electronic device may include multiple processors, each of which may be a single-core processor (single-CPU) or a multi-core processor (multi-CPU). The processor here may be a processor or a general term for multiple processing elements. For example, the processor is one or more central processing units (CPUs), or other general-purpose processors, application specific integrated circuits (ASICs), or one or more integrated circuits configured to implement an embodiment of the present invention, such as one or more microprocessors (digital signal processors, DSPs), or one or more field programmable gate arrays (field programmable gate arrays, FPGAs), or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor, etc. The processor may execute various functions of the electronic device by running or executing software programs stored in the memory and calling data stored in the memory.

[0131] In a specific implementation, as an embodiment, the processor may include one or more CPUs, such as Figure 3 The CPU0 and CPU1 shown in the figure are, of course, only exemplary.

[0132] The memory is used to store the software program for executing the solution of the present invention, and the execution is controlled by the processor. The specific implementation method can refer to the above method embodiment and will not be repeated here.

[0133] Optionally, the memory may be a read-only memory (ROM) or other types of static storage devices that can store static information and instructions, a random access memory (RAM) or other types of dynamic storage devices that can store information and instructions, or an electrically erasable programmable read-only memory (EEPROM), a compact disc read-only memory (CD-ROM) or other optical disc storage, optical disc storage (including compressed optical discs, laser discs, optical discs, digital versatile discs, Blu-ray discs, etc.), a magnetic disk storage medium or other magnetic storage device, or any other medium that can be used to carry or store the desired program code in the form of instructions or data structures and can be accessed by a computer, but is not limited thereto. The memory may be integrated with the processor or exist independently and accessed through the interface circuit ( Figure 3 (not shown) is coupled to the processor, which is not specifically limited in this embodiment of the present invention.

[0134] The transceiver may include a receiver and a transmitter ( Figure 3 The receiver is used to implement the receiving function, and the transmitter is used to implement the sending function. The transceiver can be integrated with the processor or exist independently and communicate with the electronic device through the interface circuit ( Figure 3 (not shown) is coupled to the processor, which is not specifically limited in this embodiment of the present invention.

[0135] In addition, it should be noted that Figure 3 The structure of the electronic device shown in the figure does not constitute a limitation on the device, and the actual device may include more or fewer components than shown in the figure, or combine certain components, or arrange the components differently. In addition, the technical effects achieved by the electronic device when executing the method of the first embodiment above can refer to the technical effects described in the first embodiment above, so they are not repeated here.

[0136] Third embodiment

[0137] This embodiment provides a computer-readable storage medium, which stores at least one instruction, and the instruction is loaded and executed by a processor to implement the method of the first embodiment. The computer-readable storage medium may be a ROM, a random access memory, a CD-ROM, a magnetic tape, a floppy disk, an optical data storage device, etc. The instructions stored therein may be loaded by a processor in a terminal to execute the method.

[0138] In addition, it should be noted that the present invention can be provided as a method, an apparatus or a computer program product. Therefore, the embodiment of the present invention can be in the form of a full or partial hardware embodiment, a full or partial software embodiment or an embodiment combining software and hardware. Moreover, when implemented using software, the embodiment of the present invention can be in the form of a computer program product implemented on one or more computer-usable storage media containing computer-usable program codes. The computer program product includes one or more computer instructions or computer programs. When the computer instructions or computer programs are loaded or executed on a computer, the process or function described in the embodiment of the present invention is generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable devices. The computer instructions can be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another computer-readable storage medium, for example, the computer instructions can be transmitted from one website site, computer, server or data center to another website site, computer, server or data center by wired (e.g., infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that can be accessed by a computer or a data storage device such as a server or data center containing one or more available media sets. The available medium may be a magnetic medium (eg, a floppy disk, a hard disk, a magnetic tape), an optical medium (eg, a DVD), or a semiconductor medium. The semiconductor medium may be a solid state hard disk.

[0139] The embodiments of the present invention are described with reference to the flowcharts and / or block diagrams of the methods, terminal devices (systems), and computer program products according to the embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of the processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, an embedded processor, or other programmable data processing terminal device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing terminal device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1A device that provides the functions specified in a block or multiple blocks.

[0140] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing terminal device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce a manufactured product including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 These computer program instructions can also be loaded onto a computer or other programmable data processing terminal device, so that a series of operation steps are executed on the computer or other programmable terminal device to produce a computer-implemented process, so that the instructions executed on the computer or other programmable terminal device provide for implementing the process in the process. Figure 1 A process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0141] It should also be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. The terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that the process, method, article or terminal device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or terminal device. In the absence of more restrictions, the elements defined by the sentence "including one..." do not exclude the existence of other identical elements in the process, method, article or terminal device including the elements. In addition, the term "and / or" is only an association relationship describing the associated objects, indicating that there can be three relationships, for example, A and / or B, which can represent: A exists alone, A and B exist at the same time, and B exists alone, wherein A and B can be singular or plural. In addition, the character " / " in this article generally indicates that the objects before and after are in an "or" relationship, but it may also indicate an "and / or" relationship. Please refer to the context for specific understanding. "At least one" means one or more, and "plurality" means two or more. "At least one of the following" or similar expressions refers to any combination of these items, including any combination of single or plural items. For example, at least one of a, b or c can be represented by: a, b, c, ab, ac, bc or abc, where a, b, c can be single or plural.

[0142] In addition, it can be understood that in various embodiments of the present invention, the size of the serial numbers of the above-mentioned processes 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 invention.

[0143] Those skilled in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0144] In several embodiments provided by the present invention, it should be understood that the disclosed equipment, devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic, for example, the division of functional modules / units is only a logical function division, and there may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another device, or some features can be ignored or not executed. Another point, the coupling or direct coupling or communication connection between each other shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms. The unit described as a separate component may or may not be physically separated, and the component displayed as a unit may or may not be a physical unit, that is, it may be located in one place, or it may be distributed on multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the scheme of this embodiment. In addition, each functional unit in each embodiment of the present invention can be integrated in a processing unit, or each unit can exist physically alone, or two or more units can be integrated in one unit.

[0145] If the method is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product, which is stored in a storage medium and includes several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk.

[0146] Finally, it should be noted that the above is only a preferred embodiment of the present invention. It should be pointed out that although the preferred embodiment of the present invention has been described, for ordinary technicians in this technical field, once the basic creative concept of the present invention is known, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should also be regarded as the protection scope of the present invention. Therefore, the attached claims are intended to be interpreted as including the preferred embodiment and all changes and modifications that fall within the scope of the embodiments of the present invention.

Claims

1. A laser SLAM point cloud registration method based on locatability detection, characterized in that: include: Obtain the radar point cloud to be registered and extract the plane feature points therein; Based on the extracted plane feature points, a cost function of point-to-plane ICP is established; The information matrix of the current point cloud is constructed using the cost function, and the information matrix is ​​decomposed to obtain a matrix containing only rotation information and a matrix containing only translation information, as well as the eigenvalues ​​and eigenvectors of the rotation information and translation information; Obtain the rotation information and translation information pair of each plane feature point to obtain the information vector of the current point cloud; Based on the information vector, calculate the locatability contribution value of each point and screen the plane feature points; The screened plane feature points are used to reconstruct the cost function of point-to-plane ICP, and the reconstructed cost function is optimized. The optimal pose transformation of the current point cloud is obtained through iterative solution to achieve point cloud registration.

2. The laser SLAM point cloud registration method based on locatability detection as claimed in claim 1, characterized in that: The step of obtaining the radar point cloud to be registered and extracting the plane feature points therein comprises: Obtain the radar point cloud to be registered, remove the five points at the front and back ends of each scanning line in the point cloud, and then calculate the curvature value of each remaining point; Points whose curvature values ​​are less than a first preset threshold are extracted as plane feature points.

3. The laser SLAM point cloud registration method based on locatability detection as claimed in claim 1, characterized in that: The cost function of point-to-plane ICP is established based on the extracted plane feature points, including: Transform the plane feature points from the radar coordinate system to the world coordinate system; After completing the coordinate system conversion, for each plane feature point, query the 5 feature points closest to it, construct an overdetermined equation, and calculate the plane feature corresponding to the current point through QR decomposition; Based on the plane features corresponding to each point, a cost function of point-to-plane ICP is established.

4. The laser SLAM point cloud registration method based on locatability detection as claimed in claim 1, characterized in that: The method of using the cost function to construct the information matrix of the current point cloud, decomposing the information matrix, obtaining a matrix containing only rotation information and a matrix containing only translation information, as well as eigenvalues ​​and eigenvectors of the rotation information and the translation information, includes: Linearize the rotation matrix in the cost function, the formula is: Where R represents the linearized rotation matrix; α represents the angle of rotation around the x-axis; β represents the angle of rotation around the y-axis; γ represents the angle of rotation around the z-axis; |r| × Represents the antisymmetric matrix of the rotation vector; I represents the identity matrix; Substituting the linearized rotation matrix into the expression of the cost function, we get: Among them, ε represents the cost function; Represents the position and posture of the i-th plane feature point in the world coordinate system at time k; represents the pose of the point on the feature plane corresponding to the i-th plane feature point in the world coordinate system at time k; t represents the translation vector of the pose transformation of the current point cloud from the radar coordinate system to the world coordinate system; n k,i represents the normal vector of the i-th feature plane at time k; N represents the number of feature points in the point cloud of the current frame; r represents the rotation vector; Convert the cost function solution process into a quadratic optimization problem: Among them, x k,i is the variable to be optimized; T represents the transpose of the matrix; A represents the Jacobian matrix of the optimization problem; b ′ Indicates the constraint relationship between the integrated point clouds; Const is a constant; The Hessian matrix H is approximated by the Jacobian matrix of the optimization problem; Performing principal component analysis on H, we can obtain a matrix containing only rotation information and a matrix containing only translation information, as well as the eigenvalues ​​and eigenvectors of the rotation information and translation information.

5. The laser SLAM point cloud registration method based on locatability detection as claimed in claim 1, characterized in that: The step of obtaining the rotation information and translation information pair of each plane feature point to obtain the information vector of the current point cloud includes: Construct an information vector for each plane feature point, the formula is as follows: Among them, d k,i represents the information vector of the i-th plane feature point at time k, where i = 1, 2, ..., N, and N represents the number of feature points in the current frame point cloud; represents the position and posture of the i-th plane feature point in the world coordinate system at time k; n k,i represents the normal vector of the i-th feature plane at time k; ‖‖2 represents the bi-norm of the orientation quantity.

6. The laser SLAM point cloud registration method based on locatability detection as claimed in claim 5, characterized in that: Based on the information vector, the locatability contribution value of each point is calculated, and the plane feature points are screened, including: Merge the information vectors of all points into matrices containing rotation information and a matrix containing translation information The formula is: Where T represents the transpose of the matrix; N represents the number of feature points in the point cloud of the current frame; Calculate the localizability contribution matrix of each information pair in the characteristic direction of the rotation matrix and the translation information matrix respectively and Combined with L r and L t , and obtain the localizability contribution matrix containing all dimensional information The formula is: Among them, |·| means taking the absolute value of each element in the vector; V r The eigenvector matrix representing the rotation information; V t The eigenvector matrix representing the translation information; each row vector in L represents the projection of each information vector on the eigenvector, and each element represents the localizability contribution value of the corresponding plane feature point in each degree of freedom of posture; Filter out the elements in L that are smaller than a second preset threshold value to obtain a filtered localization contribution matrix; The filtered localizability contribution matrix is ​​summarized into the localizability contribution I(j) according to each posture degree of freedom: Among them, L ′ (i,j) represents the element value of the i-th row and j-th column in the filtered localization contribution matrix; It is determined whether the locatability contribution corresponding to each degree of freedom is greater than a third preset threshold value. If it is greater than the third preset threshold value, all plane points in the locatability contribution matrix constituting the current degree of freedom are screened out.

7. The laser SLAM point cloud registration method based on locatability detection as claimed in claim 6, characterized in that: The third preset threshold is determined according to the laser radar used.

8. The laser SLAM point cloud registration method based on locatability detection as claimed in claim 1, characterized in that: The optimizing of the reconstruction cost function comprises: The Gauss-Newton iteration method is used to optimize the reconstruction cost function.

Images