Method for constructing local map of laser SLAM based on joint adjustment of feature points

Abstract

The present invention proposes a method for constructing a local map of laser SLAM based on joint adjustment of feature points. The proposed method solves problems such as poor map construction accuracy and low pose solution efficiency in the field of driverless. By jointly optimizing and solving pose parameters and world coordinate points, the error transmission during local map construction is effectively suppressed. By using the method of virtual lines to reduce the dimension of transformation parameters, the parameter search space and solution difficulty are significantly reduced, and the pose calculation efficiency is improved. The idea of rotation averaging and displacement averaging is proposed to provide a new idea for pose optimization. The invention can create a robust and high-precision local map, effectively suppress the error transmission of the SLAM system, and significantly improve the mapping accuracy while enhancing the calculation efficiency.

Description

Technical Field

[0001] The present invention belongs to the technical fields of surveying and mapping remote sensing and unmanned driving, and particularly relates to a method for constructing a local map of laser SLAM based on joint adjustment of feature points. Background Art

[0002] Positioning, navigation and mapping are basic means for obtaining spatio-temporal information, and play a very important role in the fields of national economic construction and national defense security, and have been included in the technology blockade list by the United States. The high-precision simultaneous localization and mapping (SLAM) technology is a bottleneck problem that must be solved for unmanned systems to achieve fully autonomous control, and has important application prospects and economic value.

[0003] Laser SLAM has higher mapping accuracy and stability than visual SLAM and has been commercially applied. However, for application scenarios with high precision requirements (such as high-precision maps, three-dimensional surveying and mapping, etc.), the current laser SLAM methods are still difficult to meet the requirements. One of the main reasons is that mainstream laser SLAM algorithms usually extract features by using the curvature information of laser scan lines, ignoring the geometric structure characteristics of local areas, resulting in relatively low position accuracy of matching points. At the same time, the SLAM front end uses incremental matching, and the matching error is bound to be transmitted along the chain, ultimately leading to trajectory drift. Although with the development of loop detection and graph optimization technologies, this problem has been effectively improved, it is still prominent. The graph optimization technology only constructs geometric constraints by using the relative poses between nodes, ignoring the constraints between matching connection points, essentially causing the vulnerability of the adjustment system, and the high efficiency requirement of SLAM determines that the classical block adjustment technology is not applicable. On the internationally famous evaluation dataset KITTI, the optimal position drift error of laser SLAM is still greater than 0.5%, which cannot meet the requirements of long-term high-precision for laser SLAM. Summary of the Invention

[0004] Aiming at the problem of fast local error transmission in laser simultaneous localization and mapping (SLAM), a local error elimination method combining multiple point cloud frames is developed, and the mathematical ideas of parameter space dimensionality reduction and transformation parameter averaging are studied to solve the contradiction between accuracy and speed in adjustment optimization, and effectively suppress the error transmission in local map construction. The present invention proposes a method for constructing a local map of laser SLAM based on joint adjustment of feature points. This invention draws on the successful experience of block adjustment in the field of photogrammetry, applies it to the field of three-dimensional laser point clouds, and constructs a pose optimization model by using multiply overlapping matching connection points, which can basically eliminate the error accumulation effect in the local map.

[0005] Using the local map as the matching primitive for subsequent pose graph optimization can effectively reduce the error accumulation rate and shrink the scale of the pose graph, thus achieving synchronous improvement in accuracy and efficiency, which has been verified in Google's Cartographer. However, its frame-to-model local map construction strategy is essentially an incremental matching method, which is bound to cause errors to propagate along the chain. Drawing on the successful experience of block adjustment in photogrammetry and using the overlapping matching connection points to construct a pose optimization model can basically eliminate the error accumulation effect. Although limited by the efficiency requirements of SLAM and unable to perform global block adjustment, it is completely feasible to apply it to local map construction. At the same time, due to the sparse characteristics of the laser point cloud, the position accuracy of the feature matching points still needs to be improved. Based on the above analysis, the present invention mainly proposes a method for eliminating local map errors by combining multiple point cloud frames. First, a symmetric point-plane ICP fine registration method is proposed to optimize the position accuracy of the matching points; secondly, the virtual line concept is used for parameter space decomposition and adjustment modeling of the matching connection points to reduce the solution complexity; then, rotation averaging and translation averaging models are constructed, and the global optimal solution of the combined adjustment is obtained through the convex relaxation idea to realize the generation of a high-precision local map.

[0006] To achieve the above object, the laser SLAM local map construction method based on joint adjustment of feature points designed by the present invention mainly includes the following steps:

[0007] Step 1, perform three-dimensional laser point cloud feature matching in a coarse-to-fine manner to obtain a set of high-precision three-dimensional matching points;

[0008] Step 2, construct a virtual line from the above set of matching points and perform parameter space decomposition, and use the virtual line to decompose the six-degree-of-freedom rigid body transformation in the parameter space to obtain two three-degree-of-freedom sub-problems, namely the rotation sub-problem and the translation sub-problem;

[0009] Step 3, perform adjustment modeling based on the rotation sub-problem and the translation sub-problem of the matching connection points to obtain the pose parameters of each frame of point cloud;

[0010] Step 4, obtain the optimal position and attitude parameters of each frame of point cloud based on rotation averaging and translation averaging;

[0011] Step 5, according to the final position and attitude parameters, convert the laser point cloud frames included in the local map to a unified coordinate system to obtain the local map.

[0012] Further, the specific implementation of Step 1 includes the following sub-steps;

[0013] First, for the m laser point cloud frames included in the local map two frames of point clouds S i and S j, the ISS algorithm is used to extract 3D features from each frame of point cloud, and the FPFH algorithm is used to describe these features to obtain feature description vectors. For each feature vector in S i , search for the feature vector with the closest Euclidean distance among all S j feature vectors to form one-to-one matching pairs;

[0014] Secondly, for the initial matching point set obtained in the previous step, the RANSAC method is used to calculate the relative position and pose parameters between point cloud S i and S j , that is, the rigid body transformation model parameters; using the relative position and pose parameters as the initial values, the symmetric point-plane ICP algorithm is used for fine registration to obtain the fine registration relative position and pose parameters;

[0015] Finally, for each feature point p i of point cloud S i , predict its optimal position in point cloud S j according to the fine registration relative position and pose parameters Search for the point q j in point cloud S j that is closest to the optimal position. If ε is the distance threshold, then p i and q j are considered as the fine-optimized matching pairs to obtain the matching point set.

[0016] Further, the specific implementation method of the symmetric point-plane ICP algorithm is as follows;

[0017] Given the matching point (x i , y i ) and the normal vector The mathematical definition of its symmetric point-plane distance d i (·) is:

[0018]

[0019] In the formula, i represents the number of the matching point, (R, t) are the rotation and translation parameters. Different from the point-plane distance, the symmetric distance takes into account the normal information of both frames of point clouds;

[0020] Based on the symmetric point-plane distance metric and introducing an adaptive scale M-estimation to cope with noise and gross errors, a mathematical model of symmetric point-plane ICP is constructed:

[0021]

[0022] In the formula, ρ is the robust cost function; w i is the weight function; χ ρ (w i) is the weight function penalty term.

[0023] Furthermore, the Rodriguez rotation representation method is used to linearize the mathematical model of point-plane ICP, and based on the initial value obtained by RANSAC, the Gauss-Newton method is used to solve it.

[0024] Furthermore, the specific implementation of step 2 includes the following sub-steps;

[0025] The virtual line is randomly connected by pairs of feature points. Each pair of matching points (x i , y i ) and (x j , y j ) can form a virtual line matching pair The decomposition of the virtual line parameter space utilizes its translational invariance. If (x i , y i ) and (x j , y j ) are correctly matched, then there is

[0026]

[0027] where i and j represent the numbers of the matching points, (R, t) are the rotation and translation parameters. Subtracting the two equations gives It can be seen that there is only a rotation relationship between the virtual lines , and they are translation-independent quantities. Therefore, the 6-degree-of-freedom point cloud matching problem is decomposed into two 3-degree-of-freedom sub-problems for separate solution, and the solution accuracy is not affected. At the same time, the virtual line is formed by pairing feature points in pairs, and the data volume is only half of that of the feature points.

[0028] Furthermore, the specific implementation method of step 3 is as follows;

[0029] Assume that the local map contains m laser point cloud frames whose pose parameters are The extracted matching connection points correspond to a total of n world coordinate points Then, the adjustment model based on the matching connection points is

[0030]

[0031] In the formula, X ij is the observation value of the world coordinate point in the i-th point cloud frame; f describes and the geometric relationship between X ij ; for the 6-degree-of-freedom problem, X ij is the connection point obtained by three-dimensional feature matching, f is the rigid body transformation, and L i =(R i , t i), (R, t) are rotation and translation parameters; for the 3 - degree - of - freedom sub - problem, X ij is the coordinate of the virtual line formed by the matching connection points, f is the rotation transformation or translation transformation, L i = R i or L i = t i , this adjustment model takes the pose parameters and world coordinate points as unknowns for optimization and solution, and can effectively eliminate the error accumulation effect.

[0032] Furthermore, the adjustment model is linearized by Taylor expansion and solved quickly by the Gauss - Newton method.

[0033] Furthermore, the specific implementation method of rotation averaging in step 4 is as follows;

[0034] Given a set of relative rotation observations {R ij}, where R ij represents the rotation difference between the overlapping point - cloud frames S i and S j , the goal of rotation averaging is to recover the absolute rotation pose of the point - cloud frames from {R ij} The relative rotation {R ij} and the absolute rotation can form an undirected graph structure G=(V, E), where the node V is the absolute rotation The edge E is composed of the observations {R ij}; based on the relational expression The mathematical model of rotation averaging is,

[0035]

[0036] In the formula, ||·|| F is the Frobenius norm, n is the number of the formed edges E. Different from the connection - point adjustment model, rotation averaging first obtains the relative rotation of the point - cloud frames based on the virtual line and takes it as the observation. To obtain the global optimal solution, the model (5) is relaxed by SDP (semidefinite programming) and solved by the Coordinate descent method.

[0037] Furthermore, the specific implementation method of translation averaging in step 4 is as follows;

[0038] Given a set of relative translation observations {t ij}, t ij represents the position difference between S i and S j , the goal of translation averaging is to recover the absolute position from {t ij} Similar to the rotational averaging, {t ij} can also form an undirected graph G=(V, E), where the nodes V are the absolute translations The edges E are composed of {t ij}, and the mathematical model of translational averaging is

[0039]

[0040] In the formula, is a known quantity obtained by rotational averaging. Therefore, formula (6) is a linear least squares problem and is directly solved based on the normal equation.

[0041] Furthermore, the specific implementation manner of step 5 is as follows;

[0042] Assume that the local map contains m laser point cloud frames whose pose parameters are calculated by step 4). According to the pose parameters the point cloud is transformed into a unified coordinate system to form a local map.

[0043] The present invention has the following positive effects:

[0044] 1) The present invention proposes a method for constructing a local map of laser SLAM based on joint adjustment of feature points. By jointly using pose parameters and world coordinate points as unknowns for optimization and solution, it effectively suppresses the error transmission in local map construction.

[0045] 2) The present invention proposes a method for reducing the dimension of the parameter space. Through the concept of virtuality, without loss of the accuracy of the solution, the 6-degree-of-freedom rigid body transformation parameter space is decomposed into two 3-degree-of-freedom sub-problems, greatly reducing the parameter search space and the difficulty of solution, and solving the contradictory problem of accuracy and speed in adjustment optimization.

[0046] 3) The present invention provides an idea for pose adjustment optimization using rotational averaging and translational averaging.

[0047] Adopting the present invention can construct a three-dimensional laser local map with higher precision and stability, laying a foundation for using the local map as a matching primitive for subsequent pose graph optimization, thereby effectively reducing the error accumulation speed of the SLAM system and reducing the scale of the pose graph, and achieving synchronous improvement of accuracy and efficiency. The present invention has broad application prospects in the fields of indoor mapping, three-dimensional reconstruction, autonomous driving, mine surveying, etc. Brief Description of the Drawings

[0048] Figure 1 is the technical flow chart of the present invention.

[0049] Figure 2Schematic diagram of the virtual line in the present invention.

[0050] Figure 3 Schematic diagram of the undirected graph structure based on rotation in the present invention. Detailed implementation manners

[0051] The solution of the present invention will be further described in detail below with reference to the accompanying drawings.

[0052] As Figure 1 shown, a method for constructing a local map of laser SLAM based on joint adjustment of feature points includes the following steps:

[0053] Step 1), perform three-dimensional laser point cloud feature matching in a coarse-to-fine manner to obtain a high-precision three-dimensional matching point set; first, perform rough registration of the point cloud based on three-dimensional feature detection and feature description methods, and use symmetric point-plane ICP for fine registration to optimize the position accuracy of the matching points.

[0054] Furthermore, the specific implementation manner of step 1) is as follows:

[0055] First, for any two frames of point clouds S and S i in the m laser point cloud frames j contained in the local map, with an overlap of more than 30%, use the ISS algorithm to extract three-dimensional features from each frame of point cloud, and use the FPFH algorithm to describe these features to obtain feature description vectors. For each feature vector in S i , search for the feature vector with the closest Euclidean distance among all S j feature vectors to form a one-to-one matching pair.

[0056] Secondly, for the initial matching point set obtained in the previous step, use the RANSAC method to calculate the relative position and pose parameters between point clouds S i and S j , that is, the rigid body transformation model parameters. Take the relative position and pose parameters as the initial values, and use the robust symmetric point-plane ICP algorithm proposed by the present invention for fine registration to obtain the fine registration relative position and pose parameters.

[0057] Finally, for each feature point p i of point cloud S i , predict its optimal position in point cloud S j using the fine registration relative position and pose parameters Search for the point q j in point cloud S j that is closest to the optimal position. If (ε is the distance threshold), then it is considered that p i and q jFor the refined matching pairs, a set of matching points is obtained.

[0058] The present invention improves the convergence efficiency and convergence interval of the ICP algorithm by constructing the symmetric point-plane distance. Given the matching point (x i , y i ) and the normal vector The mathematical definition of its symmetric point-plane distance d i (·) is:

[0059]

[0060] In the formula, (R, t) are the rotation and translation parameters. Different from the point-plane distance, the symmetric distance takes into account the normal information of two frames of point clouds at the same time.

[0061] Due to the sparsity of the point cloud, the position accuracy of the matching points obtained by the feature method is not high. In order to suppress the error transmission to the greatest extent, a fine registration method is used to optimize the matching point accuracy. Therefore, based on the symmetric point-plane distance metric and introducing an adaptive scale M-estimation to cope with noise and gross errors, a mathematical model of symmetric point-plane ICP is constructed:

[0062]

[0063] In the formula, ρ is a robust cost function; w i is the weight function; χ ρ (w i ) is the weight function penalty term. The present invention uses the Rodriguez rotation representation method for model linearization and, based on the good initial value obtained by RANSAC, uses the Gauss-Newton method to solve.

[0064] Step 2), as Figure 2 shown, construct a virtual line from the above set of matching points and perform parameter space decomposition, propose the concept of virtuality, and use the virtual line to decompose the 6-degree-of-freedom rigid body transformation into parameter space to obtain two three-degree-of-freedom sub-problems, namely the rotation sub-problem and the translation sub-problem;

[0065] First, randomly pair the matching points in pairs. Each pair of matching points (x i , y i ) and (x j , y j ) can form a virtual line matching pair

[0066] Secondly, there is only a rotation relationship between the virtual lines , that is According to the Gauss-Newton method, calculate the rotation matrix R i between every two frames of point clouds S j ; then R ij ; then R ijAs known numbers, the translation vector t of every two frames of point clouds is calculated from the optimized matching points ij .

[0067] Furthermore, the specific implementation of step 2) is as follows:

[0068] The virtual lines are randomly connected pairwise by feature points, and the parameter space decomposition of the virtual lines utilizes their translational invariance. If (x i , y i ) and (x j , y j ) are correctly matched (for the convenience of description, the influence of noise is not considered), then there is

[0069]

[0070] Subtracting the two equations gives It can be seen that there is only a rotational relationship between the virtual lines , and they are translation-independent quantities. Therefore, the 6-degree-of-freedom point cloud matching problem can be decomposed into two 3-degree-of-freedom sub-problems for separate solution, without affecting the solution accuracy. At the same time, the virtual lines are formed by pairwise pairing of feature points, and the data volume is only half of that of the feature points.

[0071] Step 3), based on the adjustment and optimization of the rotational sub-problem and translational sub-problem of the matching connection points, obtain the pose parameters of each frame of point cloud

[0072] Furthermore, the specific implementation of step 3) is as follows:

[0073] Assume that the local map contains m frames of lidar point clouds whose pose parameters are The extracted matching connection points correspond to a total of n world coordinate points Then, the adjustment model based on the matching connection points is

[0074]

[0075] In the formula, X ij is the observation value of the world coordinate point in the i-th point cloud frame; f describes the and X ij geometric relationship between them. For the 6-degree-of-freedom problem, X ij is the connection point obtained by three-dimensional feature matching, f is a rigid body transformation, L i =(R i , t i ); for the 3-degree-of-freedom sub-problem, X ij is the coordinate of the virtual line formed by the matching connection points (the virtual line coordinate is consistent with the point coordinate and can also be regarded as a point), f is a rotational transformation or a translational transformation, Li = R i or L i = t i This adjustment model takes pose parameters and world coordinate points as unknowns for optimization and solution together, and can effectively eliminate the error accumulation effect. The nonlinear equation is linearized by Taylor expansion, and the Gauss-Newton method is used for fast solution.

[0076] First, the rotation sub-problem of virtual line matching points is constructed from formula (4). The nonlinear equation is linearized by Taylor expansion, and the Gauss-Newton method is used to solve for the rotation transformation parameter R of each frame of point cloud in the unified coordinate system i ;

[0077] Then, taking R i as the known value, the translation sub-problem is constructed from formula (4), and the Gauss-Newton method is used to solve for the translation transformation parameter t of each frame of point cloud in the unified coordinate system i .

[0078] Step 4), as Figure 3 , obtain the optimal position and pose parameters of each frame of point cloud based on rotation averaging and translation averaging.

[0079] Furthermore, the specific implementation method of step 4) is as follows:

[0080] Rotation averaging model: Given a set of relative rotation observations {R ij}, where R ij represents the rotation difference between overlapping point cloud frames S i and S j . The goal of rotation averaging is to recover the absolute rotation pose of the point cloud frames from {R ij} The relative rotation {R ij} and the absolute rotation can form an undirected graph structure G = (V, E), where the node V is the absolute rotation and the edge E is composed of the observations {R ij}. Based on the relational expression The mathematical model of rotation averaging is

[0081]

[0082] In the formula, ||·|| Fis the Frobenius norm, and n is the number of the formed edges E. Different from the joint adjustment model, rotation averaging first obtains the relative rotation of the point cloud frames based on the virtual lines and uses it as the observation. To obtain the global optimal solution, the present invention performs SDP (semidefinite programming) relaxation on the model (5) and uses the coordinate descent method to solve it.

[0083] Translation averaging model: Given a set of relative translation observations {t ij}, t ij represents the position difference between S i and S j . The goal of translation averaging is to recover the absolute positions from {t ij}. Similar to rotation averaging, {t ij} can also form an undirected graph G=(V, E), where the nodes V are the absolute translations and the edges E are formed by {t ij}. The mathematical model of translation averaging is (rotation has been eliminated in advance),

[0084]

[0085] where, is the known quantity obtained by rotation averaging. Therefore, formula (6) is a linear least squares problem and is directly solved based on the normal equation.

[0086] First, take the relative rotations {R ij} and relative translations {t ij} obtained in step 2) as the constraint conditions, and take the attitude parameters {R i} and position parameters {t i} obtained in step 3) as the initial values.

[0087] Then, construct the mathematical model of rotation averaging according to formula (5), perform relaxation processing based on SDP, and use the coordinate descent method to solve to obtain the final attitude parameters of each frame of point cloud.

[0088] Finally, take the final attitude parameters of the previous step as the known values, construct the mathematical model of translation averaging according to formula (6), and use the least squares method to solve to obtain the final position parameters of each frame of point cloud.

[0089] In step 5), according to the final position and attitude parameters, convert the m laser point cloud frames included in the local map to the unified coordinate system to obtain the local map.

[0090] Furthermore, the specific implementation manner of step 5) is as follows:

[0091] Assume that the local map contains m laser point cloud frames whose pose parameters are calculated by step 4). According to the pose parameters the point cloud is transformed into a unified coordinate system to form a local map.

[0092] The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. For those skilled in the art, the present invention can have various changes and modifications. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for constructing a local map of laser SLAM based on joint adjustment of feature points, characterized in that It includes the following steps: Step 1: Perform three-dimensional laser point cloud feature matching in a coarse-to-fine manner to obtain a set of high-precision three-dimensional matching points; Step 2: Construct a virtual line from the above-mentioned set of matching points and perform parameter space decomposition. Use the virtual line to decompose the six-degree-of-freedom rigid body transformation into parameter space to obtain two three-degree-of-freedom sub-problems, namely the rotation sub-problem and the translation sub-problem; Step 3: Based on the rotation sub-problem and the translation sub-problem of the matching connection points, perform adjustment modeling to obtain the pose parameters of each frame of point cloud; Step 4: Based on rotation averaging and translation averaging, obtain the optimal position and attitude parameters of each frame of point cloud; The specific implementation of rotation averaging in Step 4 is as follows; Given a set of relative rotation observations {R ij}, where R ij represents the rotational difference between overlapping point cloud frames S i and S j . The goal of rotation averaging is to recover the absolute rotation poses of the point cloud frames from {R ij}. The relative rotations {R ij} and the absolute rotations can form an undirected graph structure G = (V, E), where the nodes V are the absolute rotations and the edges E are composed of the observations {R ij}; Based on the relation the mathematical model of rotation averaging is where ||·|| F is the Frobenius norm, n is the number of the formed edges E. Different from the connection point adjustment model, the rotational averaging first obtains the relative rotation of the point cloud frames based on the virtual lines and takes it as the observation. To obtain the global optimal solution, the model (5) is processed by SDP (semidefinite programming) relaxation and solved by the Coordinate descent method; The specific implementation of translation averaging in Step 4 is as follows; Given a set of relative translation observations {t ij}, where t ij represents the position difference between S i and S j . The goal of translational averaging is to recover the absolute position from {t ij}. Similar to rotational averaging, {t ij} can also form an undirected graph G = (V, E), where the nodes V are the absolute translations and the edges E are composed of {t ij}. The mathematical model of translational averaging is In the formula, is a known quantity obtained by rotational averaging. Therefore, Equation (6) is a linear least squares problem and is solved directly based on the normal equation. Step 5: According to the final position and attitude parameters, convert the laser point cloud frames included in the local map to a unified coordinate system to obtain the local map.

2. The method for constructing a local map of laser SLAM based on joint adjustment of feature points according to claim 1, wherein: The specific implementation of Step 1 includes the following sub-steps; First, for the m laser point cloud frames contained in the local map in two frames of point clouds S i and S j , use the ISS algorithm to extract three-dimensional features from each frame of point cloud, and use the FPFH algorithm to describe these features to obtain feature description vectors. For each feature vector in S i , search for the feature vector with the closest Euclidean distance among all S j feature vectors to form one-to-one matching pairs; Secondly, for the initial matching point set obtained in the previous step, use the RANSAC method to calculate the relative position and pose parameters between point cloud S i and S j That is, the rigid body transformation model parameters; take the relative position and pose parameters as the initial values, and use the symmetric point-plane ICP algorithm for fine registration to obtain the fine registration relative position and pose parameters; Finally, for each feature point p i of the point cloud S i , its optimal position in the point cloud S j is predicted by the fine registration relative position and attitude parameters Search for the point q j in the point cloud S j that is closest to the optimal position. If ε is the distance threshold, then p i and q j are considered as the fine-optimized matching pairs, and the set of matching points is obtained.

3. The method for constructing a local map of laser SLAM based on joint adjustment of feature points according to claim 2, characterized in that: The specific implementation method of the symmetric point-plane ICP algorithm is as follows; Given a matching point (x i , y i ), and a normal vector the mathematical definition of its symmetric point-plane distance d i (·) is defined as: d i (R,t) = (Rx i + t - y i )·(Rn xi + n yi ) (1) In the formula, i represents the number of the matching point, (R, t) are the rotation and translation parameters. Different from the point-plane distance, the symmetric distance takes into account the normal information of two frames of point clouds at the same time; Based on the symmetric point-plane distance metric, and introducing an adaptive scale M-estimation to deal with noise and gross errors, construct a mathematical model of symmetric point-plane ICP: where ρ is the robust cost function; w i is the weight function; χ ρ (w i ) is the weight function penalty term.

4. The method for constructing a local map of laser SLAM based on joint adjustment of feature points according to claim 3, wherein: Use the Rodriguez rotation representation method to linearize the mathematical model of symmetric point-plane ICP, and based on the initial value obtained by RANSAC, use the Gauss-Newton method to solve.

5. The method for constructing a local map of laser SLAM based on joint adjustment of feature points according to claim 1, wherein: The specific implementation of Step 2 includes the following sub-steps; The virtual lines are randomly connected pairwise by feature points. Each pair of matching point pairs (x i , y i ) and (x j , y j ) can form a virtual line matching pair The decomposition of the virtual line parameter space makes use of its translational invariance. If (x i , y i ) and (x j , y j ) are correctly matched, then where \(i\) and \(j\) represent the numbers of the matching points, \((R, t)\) are the rotation and translation parameters, and subtracting the two equations gives It can be seen that the virtual lines only have a rotation relationship between them and are independent of translation; therefore, the 6 - degree - of - freedom point cloud matching problem is decomposed into two 3 - degree - of - freedom sub - problems for separate solution, and the solution accuracy is not affected. At the same time, the virtual lines are formed by pairing feature points in pairs, and the data volume is only half of that of the feature points.

6. The method for constructing a local map of laser SLAM based on joint adjustment of feature points according to claim 1, wherein: The specific implementation method of Step 3 is as follows; Suppose the local map contains m laser point cloud frames whose pose parameters are The extracted matching connection points correspond to a total of n world coordinate points Then, the adjustment model based on the matching connection points is where X ij is the observation value of the world coordinate point in the i-th point cloud frame; f describes the geometric relationship between ij and X ij For the 6-degree-of-freedom problem, X i is the connection point obtained by 3D feature matching, f is a rigid body transformation, L i =(R i , t ij ), (R, t) are the rotation and translation parameters; for the 3-degree-of-freedom sub-problem, X i is the virtual line coordinate formed by the matching connection points, f is a rotation transformation or a translation transformation, L i =R i or L i =t , this adjustment model jointly optimizes and solves the pose parameters and the world coordinate points as unknowns, and can effectively eliminate the error accumulation effect.

7. The method for constructing a local map of laser SLAM based on joint adjustment of feature points according to claim 6, characterized in that: Perform Taylor expansion linearization on the adjustment model and use the Gauss-Newton method to quickly solve.

8. The method for constructing a local map of laser SLAM based on joint adjustment of feature points according to claim 1, characterized in that: The specific implementation method of Step 5 is as follows; Suppose the local map contains m laser point cloud frames Its pose parameters Are calculated by step 4). According to the pose parameters The point cloud Is transformed into a unified coordinate system to form a local map.

Images