SLAM factor graph optimization method, device and equipment based on plane information

Abstract

The present invention relates to the field of image processing technology, and discloses a SLAM factor graph optimization method, device and equipment based on plane information. By extracting ground information in an actual mapping environment, the plane information is used as a constraint to participate in the front-end odometer solution and the back-end global map optimization. The online solution introduces ground information to provide gravity direction constraints, which can reduce the error of posture estimation, more accurately constrain the pitch and roll posture in the global coordinate system, and reduce the deviation of global information estimation caused by the cumulative error as the odometer moves. A local point cloud map is generated based on the result of the online back-end global map optimization solution. After the online back-end pose graph optimization solution, the cumulative error of the odometer can be preliminarily eliminated. The accuracy of plane information extraction is improved under the premise that the global point cloud map basically meets the consistency. On this basis, adding rich plane information constraints is helpful to improve the global consistency of the SLAM system.

Description

Technical Field

[0001] The present invention relates to the technical field of image processing, and in particular to a SLAM factor graph optimization method, device and equipment based on plane information. Background Art

[0002] In the field of autonomous driving and robotics, multi-sensor fusion is a key direction of current SLAM research. LiDAR is usually fused with other sensors such as cameras, inertial measurement units (IMUs), GPS, etc. to improve the robustness and accuracy of environmental perception, enabling the system to work in a wider range of scenarios and complex environments, and to achieve pose estimation of the mobile carrier's motion process and construction of scene point cloud maps, thereby realizing the survey and modeling of the actual scene environment.

[0003] Among the patents and technologies for SLAM combined with plane information applications that are currently disclosed, most of them are based on ground constraint applications. For example, the patent with publication number CN115265523A proposes a method, device and readable medium for simultaneous positioning and mapping of robots. The scheme uses random sampling consistency to extract ground information from the local map point cloud of the lidar to form a ground constraint factor to participate in the factor graph optimization. This method does not solve the problem of deviation in global ground information estimation caused by the accumulated error as the odometer moves. The patent with publication number CN116734838A proposes a high-speed large-scene mapping method based on LiDAR-IMU-GNSS. It specifically discloses that in the process of SLAM front-end solution, the current frame point cloud and the local map point cloud are respectively extracted from the ground, and the constraint residual between the ground plane and the ground plane is formed to participate in the solution of the front-end pose estimation, but the ground information is not involved in the back-end optimization solution to provide a constraint effect. Summary of the invention

[0004] In view of the deficiencies in the above-mentioned prior art, the present invention provides a SLAM factor graph optimization method, device and equipment based on plane information, which uses plane information as a constraint to participate in the front-end odometer solution and the back-end global graph optimization, thereby improving the accuracy of pose graph optimization.

[0005] To achieve the above object, an embodiment of the present invention provides a SLAM factor graph optimization method based on plane information, comprising the following steps:

[0006] Step 1: Obtain a single-frame point cloud of the laser radar and extract the ground point cloud, and fit the ground vector corresponding to the frame point cloud;

[0007] Step 2, obtaining the IMU sensor measurement data between two adjacent point cloud frame sampling moments and the point cloud data at each sampling moment, and performing IMU pre-integration on the IMU sensor measurement data and constructing an IMU pre-integration residual;

[0008] Step 3, matching and solving the laser radar single-frame point cloud with its corresponding adjacent local point cloud map to obtain the point cloud matching constraint residual, fusing the point cloud matching constraint residual with the IMU pre-integration residual and performing a joint solution to obtain the pose node of the point cloud frame to be solved in the world coordinate system;

[0009] Step 4, constructing adjacent pose edge constraint residuals, loop pose edge constraint residuals, and GPS constraint residuals according to the pose nodes of the point cloud frame to be solved in the world coordinate system, and introducing gravity constraint residuals to constrain the relative pose of the pose node and the ground vector to complete the online pose graph optimization;

[0010] Step 5, dividing the optimized pose nodes into multiple trajectory segments according to a fixed-length time interval, transforming the point cloud frames of each trajectory segment and splicing them to generate a corresponding local point cloud map;

[0011] Step 6, fitting the ground and facade of the local point cloud map to form plane nodes, and adding the plane nodes to the factor graph;

[0012] Step 7, constructing an error term according to the ground vector and the plane node to form a constraint factor between the key frame pose node and the plane node, and adding the constraint factor to the factor graph;

[0013] Step 8, construct a total error function based on the factor graph, use all key frame pose nodes and plane nodes as optimization variables, optimize and solve to obtain the values ​​of all variables, transform and splice the point clouds corresponding to each frame according to the optimized pose results to generate a globally consistent point cloud map.

[0014] To achieve the above object, an embodiment of the present invention further provides a SLAM factor graph optimization device based on plane information, the device comprising:

[0015] The ground point cloud extraction module is used to obtain a single-frame point cloud of the laser radar and extract the ground point cloud, and fit the ground vector corresponding to the frame point cloud;

[0016] A data acquisition module is used to acquire the IMU sensor measurement data between two adjacent point cloud frame sampling moments and the point cloud data at each sampling moment, and to perform IMU pre-integration on the IMU sensor measurement data and construct an IMU pre-integration residual;

[0017] A data fusion module is used to match and solve the laser radar single-frame point cloud with its corresponding adjacent local point cloud map to obtain a point cloud matching constraint residual, fuse the point cloud matching constraint residual with the IMU pre-integration residual and perform a joint solution to obtain a pose node of the point cloud frame to be solved in the world coordinate system;

[0018] A constraint residual construction module is used to construct adjacent pose edge constraint residuals, loop pose edge constraint residuals, and GPS constraint residuals according to the pose nodes of the point cloud frame to be solved in the world coordinate system, and introduce gravity constraint residuals to constrain the relative pose of the pose node and the ground vector to complete the online pose graph optimization;

[0019] A local point cloud map generation module is used to divide the optimized pose nodes into multiple trajectory segments according to a fixed-length time interval, transform the point cloud frames of each trajectory segment, and then splice them to generate a corresponding local point cloud map;

[0020] A plane node generation module, used for fitting the ground and facade of the local point cloud map to form a plane node, and adding the plane node to the factor graph;

[0021] A constraint factor generation module, used for constructing an error term according to the ground vector and the plane node to form a constraint factor between the key frame pose node and the plane node, and adding the constraint factor to a factor graph;

[0022] The global point cloud map generation module is used to construct a total error function based on the factor graph, take all key frame pose nodes and plane nodes as optimization variables, optimize and solve to obtain the values ​​of all variables, transform and splice the point clouds corresponding to each frame according to the optimized pose results, and generate a globally consistent point cloud map.

[0023] To achieve the above object, an embodiment of the present invention further provides a device, wherein the device is provided with:

[0024] Memory, used to store programs;

[0025] A processor is used to execute the program stored in the memory. When the program is executed, the processor is used to execute part or all of the steps of the above method.

[0026] The embodiments of the present invention have the following beneficial technical effects:

[0027] The SLAM factor graph optimization method, device and equipment based on plane information provided by the embodiment of the present invention extracts ground information in the actual mapping environment, and uses the plane information as a constraint to participate in the front-end odometer solution and the back-end global map optimization. The online solution introduces ground information to provide gravity direction constraints, which can reduce the error of posture estimation, more accurately constrain the pitch and roll posture in the global coordinate system, and reduce the deviation of global information estimation caused by the cumulative error as the odometer moves. A local point cloud map is generated based on the result of the online back-end global map optimization solution. After the online back-end pose graph optimization solution, the cumulative error of the odometer can be preliminarily eliminated. The accuracy of plane information extraction is improved under the premise that the global point cloud map basically meets the consistency. On this basis, adding rich plane information constraints helps to improve the global consistency of the SLAM system. The structured information of the environmental point cloud can be used to help improve the accuracy of pose graph optimization, and a more refined point cloud map environment model can be obtained. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. 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 the structures shown in these drawings without paying creative work.

[0029] Figure 1 This is a flow chart of a SLAM factor graph optimization method based on plane information in Embodiment 1 of the present invention;

[0030] Figure 2 It is a structural block diagram of a SLAM factor graph optimization device based on plane information in Embodiment 2 of the present invention;

[0031] Figure 3 This is a partial structural block diagram of the device in Embodiment 3 of the present invention.

[0032] The realization of the purpose, functional features and advantages of the present invention will be further explained in conjunction with embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0033] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0034] In addition, the technical solutions between the various embodiments of the present invention can be combined with each other, but it must be based on the fact that ordinary technicians in the field can implement it. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0035] Embodiment 1

[0036] refer to Figure 1 In order to reduce the cumulative error of the online SLAM system in response to the needs of long-term operation and large-scale environments, this embodiment provides a SLAM factor graph optimization method based on plane information, introduces more effective loop detection and back-end optimization technology to ensure the accuracy of the pose results estimated by SLAM and the global point cloud map generated by splicing. Figure 1 The method in this embodiment includes the following steps 1 to 8:

[0037] Step 1: Obtain a single-frame point cloud of the laser radar and extract the ground point cloud from it, and fit the ground vector corresponding to the frame point cloud; the elevation-based region growing method is used in the process of extracting the ground point cloud;

[0038] Step 2, obtaining the IMU sensor measurement data between two adjacent point cloud frame sampling moments and the point cloud data at each sampling moment, and performing IMU pre-integration on the IMU sensor measurement data and constructing an IMU pre-integration residual;

[0039] Step 3, matching and solving the laser radar single-frame point cloud with its corresponding adjacent local point cloud map to obtain the point cloud matching constraint residual, fusing the point cloud matching constraint residual with the IMU pre-integration residual and performing a joint solution to obtain the pose node of the point cloud frame to be solved in the world coordinate system;

[0040] Step 4, according to the pose nodes of the point cloud frame to be solved in the world coordinate system, adjacent pose edge constraint residuals, loop pose edge constraint residuals, and GPS constraint residuals are respectively constructed, and gravity constraint residuals are introduced to constrain the relative pose of the pose node and the ground vector to complete the online pose graph optimization; in the horizontal ground scene, the ground vector is in the same direction as the gravity vector; the loop pose edge constraint and GNSS constraint can preliminarily eliminate a large range of accumulated errors, and this step can improve the pose estimation accuracy of the fused IMU;

[0041] Step 5, dividing the optimized pose nodes into multiple trajectory segments according to a fixed-length time interval, transforming the point cloud frames of each trajectory segment according to their corresponding pose results and splicing them to generate a corresponding local point cloud map;

[0042] Step 6, fitting the ground and facade of the local point cloud map to form plane nodes, and adding the plane nodes to the factor graph;

[0043] Step 7, constructing an error term according to the ground vector and the plane node to form a constraint factor between the key frame pose node and the plane node, and adding the constraint factor to the factor graph;

[0044] Step 8, construct a total error function based on the factor graph, use all key frame pose nodes and plane nodes as optimization variables, optimize and solve to obtain the values ​​of all variables, transform and splice the point clouds corresponding to each frame according to the optimized pose results to generate a globally consistent point cloud map.

[0045] In one embodiment, in step 1, the step of obtaining a single-frame point cloud of a laser radar and extracting a ground point cloud from the point cloud, and fitting the ground vector corresponding to the frame point cloud, comprises:

[0046] Step 11, extracting ground point cloud from single-frame point cloud of laser radar using region growing method based on elevation;

[0047] Step 12, selecting a point close to the prior ground height position from the plurality of laser radar single-frame point clouds as a seed point, iterating other points to cluster the points whose elevation differences are within a preset threshold range;

[0048] Step 13: Use the least squares method to fit the ground equation to obtain the ground vector in is the ground normal vector in the local coordinate system of key frame i, d i It is the intercept of the ground in the local coordinate system. The ground vector of each frame is recorded for use in the following steps.

[0049] In one embodiment, in step 3, fusing the point cloud matching constraint residual and the IMU pre-integration residual and performing a joint solution includes:

[0050] Step 31, define the residual equation:

[0051]

[0052] In the formula, F(T i ) is the overall objective function, T i is the pose node of the point cloud frame i to be solved in the world coordinate system, and i is the serial number of the laser radar frame corresponding to the sampling time;

[0053] α is the weight of the point cloud matching constraint residual, The pose constraint residual constructed based on the criterion of minimizing the point-to-plane distance between the i-th frame lidar point cloud and its corresponding local point cloud map;

[0054] β is the weight of the IMU pre-integration residual constructed corresponding to the sampling time of the lidar point cloud of the i-1th frame and the i-th frame, is the corresponding IMU pre-integration residual;

[0055] θ is the weight of the gravity constraint residual of the i-th frame lidar point cloud, is the ground normal vector and the direction of gravity The residual, e p (·) is the error function of the two-vector parallel constraint; where,

[0056]

[0057] Step 32: Use the Gauss-Newton method to solve the total objective function and obtain the pose node T at time i i .

[0058] In one embodiment, step 4 further comprises:

[0059] Step 41, combining the adjacent pose edge constraint residuals, the loop pose edge constraint residuals, and the GPS constraint residuals to construct the total residual equation for online backend solution:

[0060]

[0061] Where F(T) is the total objective function, T is the pose node of the point cloud frame to be solved in the world coordinate system, T = {T0, T1, …, T N}, n, m are the serial numbers of the two point cloud frames that are associated through online loop closure detection to form the loop closure pose edge constraint; It is the residual of adjacent pose edge constraints of two adjacent point cloud frames; The relative pose of the two loop point cloud frames obtained by loop closure matching constitutes the loop pose edge constraint residual. The valid GNSS pose observations at the time corresponding to key frame i constitute the GPS constrained residual;

[0062] Step 42, using the Gauss-Newton method to optimize and solve the above total residual equation, and finally solving the pose optimization results of all point cloud frames after introducing gravity constraints, adjacent constraints, loop constraints and GPS constraints to correct the cumulative errors.

[0063] In one embodiment, step 5 further comprises:

[0064] Step 51, using the elevation region growing method to fit the plane existing in the local point cloud map, to obtain multiple possible plane equations;

[0065] Step 52: According to the normal vector direction of the plane, filter out the normal vector parallel to the XY plane, corresponding to the vertical elevation plane, and then filter out the normal vector perpendicular to the XY plane, corresponding to the parallel ground plane, to obtain the elevation vectors that meet the preset point cloud quantity threshold. and the ground vector Use them as optimization variables to form the facade plane nodes participating in the factor graph optimization and ground plane nodes

[0066] In one embodiment, the plane nodes in step 6 include ground plane nodes and facade plane nodes, and the ground vector and the facade vector are both plane vectors;

[0067] Step 6 further comprises:

[0068] Step 61, saving the association relationship between each point in the local point cloud map and the corresponding point cloud frame number;

[0069] Step 62, based on the association relationship between the point clouds of the facade plane and the ground plane and the corresponding point cloud frame numbers, determine whether the number of point clouds corresponding to frame i in all associated point clouds exceeds a preset number threshold. If so, it is considered that the pose node T corresponding to frame i i With plane node There is a constraint relationship, and the constraint residual of the pose node and the plane node is constructed:

[0070]

[0071] Where D(·) is the error between two plane vectors, is the plane vector representation in the local coordinate system of frame i, is a uniform representation of a plane vector.

[0072] Furthermore, the method further includes between step 6 and step 7:

[0073] Construct plane node prior constraints:

[0074] For plane nodes, due to the inherent property that the facade is perpendicular to the ground, the normal vector of the facade plane node should have a priori constraints of being perpendicular to the XY plane, and the normal vector of the ground plane node should have a priori constraints of being parallel to the XY plane (normal vector ). Therefore, the normal vector direction of the plane node can be constrained by the residual term according to the prior constraints:

[0075]

[0076] in, is the residual of the prior constraints on the ground plane nodes, is the residual of the prior constraints on the facade plane nodes, e p (·) is the error function of the parallel constraint between two vectors, e v (·) is the error function of the two vectors’ perpendicular constraint;

[0077] Construct the ground plane node relative constraint:

[0078] For the ground plane nodes formed by two adjacent local point cloud maps, the ground corresponding to the two ground plane nodes is actually the same plane, that is, the vector representations of the two adjacent ground plane nodes are consistent, thereby constructing a constrained residual for the two ground plane nodes that have a co-viewing relationship in the local point cloud space:

[0079]

[0080] Where D(·) is the error between two plane vectors, h j ,h k are the ground plane node variables of two nodes that have a common view relationship, is the corresponding normal vector, is the corresponding intercept.

[0081] In one embodiment, in step 7, constructing an error term according to the ground vector and the plane node to form a constraint factor between the key frame pose node and the plane node, and adding the constraint factor to the factor graph includes:

[0082] Constructing an error term according to the ground vector and the ground plane node to form a constraint factor between the key frame pose node and the ground plane node and adding it to the factor graph;

[0083] An error term is constructed based on the facade plane node and the key frame pose node to form a constraint factor between the key frame pose node and the facade plane node and add it to the factor graph.

[0084] In one embodiment, in step 8, the total error function is constructed based on the factor graph, all key frame pose nodes and plane nodes are used as optimization variables, all variable values ​​are obtained by optimization, and the point clouds corresponding to each frame are transformed and spliced ​​according to the optimized pose results to generate a globally consistent point cloud map, including:

[0085] Construct the total error function equation based on the factor graph:

[0086]

[0087] Where T is all point cloud frame pose nodes, H is all ground plane nodes, and V is all facade plane nodes; where T = {T0, T1, …, T N}, H={H0,H1,…,H P},V={V0,V1,…,V Q};

[0088] With the goal of minimizing the sum of the total error function, the Gauss-Newton method is used to optimize and solve the total error function equation. All key frame pose nodes and plane nodes are used as optimization variables. Finally, the values ​​of all variables after the introduction of plane information constraints are solved. The optimized pose node values ​​are taken as the final results. The corresponding point clouds of each frame are transformed and spliced ​​according to the optimized pose results to obtain a globally consistent point cloud map.

[0089] The SLAM factor graph optimization method based on plane information in this embodiment is based on the factor graph optimization model. When extracting ground information in the actual mapping environment, the plane information is used as a constraint to participate in the front-end odometer solution and the back-end global map optimization. The online solution introduces ground information to provide gravity direction constraints to reduce the error of posture estimation, more accurately constrain the pitch and roll posture in the global coordinate system, and reduce the deviation of global information estimation caused by the cumulative error as the odometer moves. Based on the results of the online back-end global graph optimization solution, a local point cloud map is generated. After the online back-end pose graph optimization solution, the cumulative error of the odometer can be preliminarily eliminated. The accuracy of plane information extraction is improved under the premise that the global point cloud map basically meets the consistency. On this basis, adding rich plane information constraints helps to improve the global consistency of the SLAM system. The structured information of the environmental point cloud can be used to help improve the accuracy of pose graph optimization and obtain a more refined point cloud map environment model.

[0090] Combined with the structural information in the actual environment, sensors such as LiDAR can be used to detect planes such as the ground or walls in the environment, which can be detected through plane fitting or segmentation algorithms. Plane information can be used to constrain the movement of the robot and improve the accuracy of modeling the robot's movement.

[0091] Embodiment 2

[0092] Based on the SLAM factor graph optimization method based on plane information in the first embodiment, this embodiment discloses a SLAM factor graph optimization device based on plane information. Figure 2 The SLAM factor graph optimization device based on plane information includes a ground point cloud extraction module 201, a data acquisition module 202, a data fusion module 203, a constraint residual construction module 204, a local point cloud map generation module 205, a plane node generation module 206, a constraint factor generation module 207, and a global point cloud map generation module 208. The SLAM factor graph optimization device based on plane information is used to perform some or all steps of the SLAM factor graph optimization method based on plane information in Example 1. Specifically:

[0093] The ground point cloud extraction module 201 is used to obtain a single-frame point cloud of the laser radar and perform ground point cloud extraction on it, and fit the ground vector corresponding to the frame point cloud;

[0094] The data acquisition module 202 is used to acquire the IMU sensor measurement data between two adjacent point cloud frame sampling moments and the point cloud data at each sampling moment, and perform IMU pre-integration on the IMU sensor measurement data and construct an IMU pre-integration residual;

[0095] The data fusion module 203 is used to match and solve the laser radar single-frame point cloud with its corresponding adjacent local point cloud map to obtain the point cloud matching constraint residual, fuse the point cloud matching constraint residual with the IMU pre-integration residual and perform a joint solution to obtain the pose node of the point cloud frame to be solved in the world coordinate system;

[0096] The constraint residual construction module 204 is used to construct adjacent pose edge constraint residuals, loop pose edge constraint residuals, and GPS constraint residuals according to the pose nodes of the point cloud frame to be solved in the world coordinate system, and introduce gravity constraint residuals to constrain the relative pose of the pose node and the ground vector to complete the online pose graph optimization;

[0097] The local point cloud map generation module 205 is used to divide the optimized pose nodes into multiple trajectory segments according to a fixed-length time interval, transform the point cloud frames of each trajectory segment, and then splice them to generate a corresponding local point cloud map;

[0098] The plane node generation module 206 is used to fit the ground and the facade of the local point cloud map to form a plane node, and add the plane node to the factor graph;

[0099] The constraint factor generation module 207 is used to construct an error term according to the ground vector and the plane node to form a constraint factor between the key frame pose node and the plane node, and add the constraint factor to the factor graph;

[0100] The global point cloud map generation module 208 is used to construct a total error function based on the factor graph, take all key frame pose nodes and plane nodes as optimization variables, optimize and solve to obtain the values ​​of all variables, transform and splice the point clouds corresponding to each frame according to the optimized pose results, and generate a globally consistent point cloud map.

[0101] In this embodiment, the specific working process and working principle of the ground point cloud extraction module 201, the data acquisition module 202, the data fusion module 203, the constraint residual construction module 204, the local point cloud map generation module 205, the plane node generation module 206, the constraint factor generation module 207, and the global point cloud map generation module 208 are the same as the method in Example 1, so they will not be repeated in this embodiment.

[0102] Embodiment 3

[0103] like Figure 3 The figure shows a partial structural block diagram of a device disclosed in this embodiment, which may be a computer device, on which a receiver 301, a transmitter 302, a processor 303 and a memory 304 are provided. Among them, the transmitter 302 is used to send instructions and data, the receiver 301 is used to receive instructions and data, the memory 304 is used to store computer execution instructions, and the processor 303 is used to execute the computer execution instructions stored in the memory 304 to implement some or all of the steps performed by the SLAM factor graph optimization method based on plane information in the above-mentioned embodiment 1. Its specific implementation process is the same as the SLAM factor graph optimization method based on plane information in the above-mentioned embodiment 1, and will not be repeated here.

[0104] It should be noted that the memory 304 can be independent or integrated with the processor 303. When the memory 304 is independently provided, a bus is also provided on the device to connect the memory 304 and the processor 303.

[0105] Embodiment 4

[0106] This embodiment discloses a computer-readable storage medium, in which computer-executable instructions are stored. When a processor executes the computer-executable instructions, some or all of the steps executed by the SLAM factor graph optimization method based on plane information in the above-mentioned embodiment 1 are implemented.

[0107] The above description is only a preferred embodiment of the present invention, and does not limit the patent scope of the present invention. All equivalent structural changes made by using the contents of the present invention specification and drawings under the inventive concept of the present invention, or directly / indirectly applied in other related technical fields are included in the patent protection scope of the present invention.

Claims

1. A SLAM factor graph optimization method based on plane information, characterized in that: The steps include: Step 1: Obtain a single-frame point cloud of the laser radar and extract the ground point cloud, and fit the ground vector corresponding to the frame point cloud; Step 2, obtaining the IMU sensor measurement data between two adjacent point cloud frame sampling moments and the point cloud data at each sampling moment, and performing IMU pre-integration on the IMU sensor measurement data and constructing an IMU pre-integration residual; Step 3, matching and solving the laser radar single-frame point cloud with its corresponding adjacent local point cloud map to obtain the point cloud matching constraint residual, fusing the point cloud matching constraint residual with the IMU pre-integration residual and performing a joint solution to obtain the pose node of the point cloud frame to be solved in the world coordinate system; Step 4, constructing adjacent pose edge constraint residuals, loop pose edge constraint residuals, and GPS constraint residuals according to the pose nodes of the point cloud frame to be solved in the world coordinate system, and introducing gravity constraint residuals to constrain the relative pose of the pose node and the ground vector to complete the online pose graph optimization; Step 5, dividing the optimized pose nodes into multiple trajectory segments according to a fixed-length time interval, transforming the point cloud frames of each trajectory segment and splicing them to generate a corresponding local point cloud map; Step 6, fitting the ground and facade of the local point cloud map to form plane nodes, and adding the plane nodes to the factor graph; Step 7, constructing an error term according to the ground vector and the plane node to form a constraint factor between the key frame pose node and the plane node, and adding the constraint factor to the factor graph; Step 8, construct a total error function based on the factor graph, use all key frame pose nodes and plane nodes as optimization variables, optimize and solve to obtain the values ​​of all variables, transform and splice the point clouds corresponding to each frame according to the optimized pose results to generate a globally consistent point cloud map.

2. The SLAM factor graph optimization method based on plane information according to claim 1, wherein In step 1, a single-frame point cloud of a laser radar is obtained and ground point cloud is extracted, and a ground vector corresponding to the frame point cloud is obtained by fitting; include: Step 11, extracting ground point cloud from single-frame point cloud of laser radar using region growing method based on elevation; Step 12, selecting a point close to the prior ground height position from the plurality of laser radar single-frame point clouds as a seed point, iterating other points to cluster the points whose elevation differences are within a preset threshold range; Step 13, use the least squares method to fit the ground equation to the clustered point cloud to obtain the ground vector.

3. The SLAM factor graph optimization method based on plane information according to claim 2, characterized in that, In step 3, the fusing of the point cloud matching constraint residual and the IMU pre-integration residual and performing a joint solution includes: Step 31, define the residual equation: In the formula, F(T i ) is the overall objective function, T i is the pose node of the point cloud frame i to be solved in the world coordinate system, and i is the serial number of the laser radar frame corresponding to the sampling time; α is the weight of the point cloud matching constraint residual, The pose constraint residual constructed based on the criterion of minimizing the point-to-plane distance between the i-th frame lidar point cloud and its corresponding local point cloud map; β is the weight of the IMU pre-integration residual constructed corresponding to the sampling time of the lidar point cloud of the i-1th frame and the i-th frame, is the corresponding IMU pre-integration residual; θ is the weight of the gravity constraint residual of the i-th frame lidar point cloud, is the ground normal vector and the direction of gravity The residual, e p (·) is the error function of the two-vector parallel constraint; where, Step 32: Use the Gauss-Newton method to solve the total objective function and obtain the pose node T at time i i .

4. The SLAM factor graph optimization method based on plane information according to claim 3, characterized in that, Step 4 further includes: Step 41, construct the total residual equation for online backend solution: Where F(T) is the total objective function, T is the pose node of the point cloud frame to be solved in the world coordinate system, T = {T0, T1, …, T N }, n, m are the serial numbers of the two point cloud frames that constitute the loop closure pose edge constraint; It is the residual of adjacent pose edge constraints of two adjacent point cloud frames; is the residual of the loop-closed pose edge constraint of two loop-closed point cloud frames, is the GPS constraint residual at the corresponding time of key frame i; Step 42, using the Gauss-Newton method to optimize and solve the above total residual equation, and obtain the pose optimization results of all point cloud frames after introducing gravity constraints, neighbor constraints, loop constraints and GPS constraints to correct the accumulated errors.

5. The SLAM factor graph optimization method based on plane information according to claim 1, characterized in that, Step 5 further includes: Step 51, using the elevation region growing method to fit the plane where the local point cloud map exists; Step 52: According to the normal vector direction of the plane, filter out the normal vector parallel to the XY plane, corresponding to the vertical elevation plane, and then filter out the normal vector perpendicular to the XY plane, corresponding to the parallel ground plane, to obtain the elevation vectors that meet the preset point cloud quantity threshold. and the ground vector Use them as optimization variables to form the facade plane nodes participating in the factor graph optimization and ground plane nodes 6. The SLAM factor graph optimization method based on plane information according to claim 5, characterized in that, The plane nodes in step 6 include ground plane nodes and facade plane nodes, and the ground vector and the facade vector are both plane vectors; Step 6 further comprises: Step 61, saving the association relationship between each point in the local point cloud map and the corresponding point cloud frame number; Step 62, based on the association relationship between the point clouds of the facade plane and the ground plane and the corresponding point cloud frame numbers, determine whether the number of point clouds corresponding to frame i in all associated point clouds exceeds a preset number threshold. If so, it is considered that the pose node T corresponding to frame i i With plane node There is a constraint relationship, and the constraint residual of the pose node and the plane node is constructed: Where D(·) is the error between two plane vectors, is the plane vector representation in the local coordinate system of frame i, is a uniform representation of a plane vector.

7. The SLAM factor graph optimization method based on plane information according to claim 6, characterized in that, In step 7, an error term is constructed according to the ground vector and the plane node to form a constraint factor between the key frame pose node and the plane node, and the constraint factor is added to the factor graph; including: Constructing an error term according to the ground vector and the ground plane node to form a constraint factor between the key frame pose node and the ground plane node and adding it to the factor graph; An error term is constructed based on the facade plane node and the key frame pose node to form a constraint factor between the key frame pose node and the facade plane node and add it to the factor graph.

8. The SLAM factor graph optimization method based on plane information according to claim 7, characterized in that, In step 8, the total error function is constructed based on the factor graph, all key frame pose nodes and plane nodes are used as optimization variables, all variable values ​​are obtained by optimization, and the point clouds corresponding to each frame are transformed and spliced ​​according to the optimized pose results to generate a globally consistent point cloud map; include: Construct the total error function equation based on the factor graph: Where T is all point cloud frame pose nodes, H is all ground plane nodes, and V is all facade plane nodes; With the goal of minimizing the sum of the total error function, the Gauss-Newton method is used to optimize and solve the total error function equation. All key frame pose nodes and plane nodes are used as optimization variables. Finally, the values ​​of all variables are solved and the optimized pose node values ​​are taken as the final result. The corresponding point clouds of each frame are transformed and spliced ​​according to the optimized pose results to obtain a globally consistent point cloud map.

9. A SLAM factor graph optimization device based on plane information, characterized in that: The method according to any one of claims 1 to 8 is used to optimize the SLAM factor graph, wherein the device comprises: The ground point cloud extraction module is used to obtain a single-frame point cloud of the laser radar and extract the ground point cloud, and fit the ground vector corresponding to the frame point cloud; A data acquisition module is used to acquire the IMU sensor measurement data between two adjacent point cloud frame sampling moments and the point cloud data at each sampling moment, and to perform IMU pre-integration on the IMU sensor measurement data and construct an IMU pre-integration residual; A data fusion module is used to match and solve the laser radar single-frame point cloud with its corresponding adjacent local point cloud map to obtain a point cloud matching constraint residual, fuse the point cloud matching constraint residual with the IMU pre-integration residual and perform a joint solution to obtain a pose node of the point cloud frame to be solved in the world coordinate system; A constraint residual construction module is used to construct adjacent pose edge constraint residuals, loop pose edge constraint residuals, and GPS constraint residuals according to the pose nodes of the point cloud frame to be solved in the world coordinate system, and introduce gravity constraint residuals to constrain the relative pose of the pose node and the ground vector to complete the online pose graph optimization; A local point cloud map generation module is used to divide the optimized pose nodes into multiple trajectory segments according to a fixed-length time interval, transform the point cloud frames of each trajectory segment, and then splice them to generate a corresponding local point cloud map; A plane node generation module, used for fitting the ground and the facade of the local point cloud map to form a plane node, and adding the plane node to the factor graph; A constraint factor generation module, used for constructing an error term according to the ground vector and the plane node to form a constraint factor between the key frame pose node and the plane node, and adding the constraint factor to a factor graph; The global point cloud map generation module is used to construct a total error function based on the factor graph, take all key frame pose nodes and plane nodes as optimization variables, optimize and solve to obtain the values ​​of all variables, transform and splice the point clouds corresponding to each frame according to the optimized pose results, and generate a globally consistent point cloud map.

10. A device, characterized in that: The device is provided with: Memory, used to store programs; A processor, configured to execute the program stored in the memory; when the program is executed, the processor is configured to execute part or all of the steps of the method according to any one of claims 1 to 8.

Images