A laser positioning and mapping method and system based on slope factor

Abstract

The present invention relates to the field of positioning and mapping technology, and provides a slope factor-based laser positioning and mapping method and system. The method comprises: acquiring point cloud data and segmenting the point cloud data into a ground point cloud and a non-ground point cloud; performing point cloud registration based on the non-ground point cloud to obtain an odometry factor and add it to a factor graph; performing slope estimation based on the ground point cloud to add the slope factor to the factor graph; estimating the robot's pose based on the factor graph; performing loop detection based on the estimated pose to add the loop factor to the factor graph; correcting the pitch angle in the estimated pose; and forming a three-dimensional point cloud map based on the corrected robot pose. This method effectively reduces positioning error and distortion in the generated map.

Description

Technical Field

[0001] The present invention belongs to the technical field of positioning and mapping, and in particular relates to a laser positioning and mapping method and system based on a slope factor. Background Art

[0002] The statements in this section merely provide background information related to the present invention and do not necessarily constitute prior art.

[0003] Simultaneous Localization and Mapping (SLAM), a core function for autonomous robot operation, primarily encompasses two main branches: visual SLAM and laser SLAM. Laser SLAM algorithms are primarily targeted at medium- to large-scale outdoor environments, enabling robots to determine their position and achieve precise positioning. Furthermore, they can create 3D point cloud maps, enabling robots to perceive their surroundings.

[0004] Traditional laser SLAM algorithms have the following technical difficulties:

[0005] (1) There are serious elevation errors. In traditional laser SLAM, laser alignment is prone to elevation errors when running on rugged slopes. Over time, the errors gradually accumulate and are difficult to eliminate.

[0006] (2) The established 3D point cloud map is distorted. The traditional laser SLAM algorithm’s inaccurate depiction of slopes and accumulated elevation errors cause the map to have a certain degree of tilt and distortion.

[0007] (3) Lack of real-time posture correction. Traditional laser SLAM algorithms require loop detection to correct the robot's posture. In real scenarios, the interval between loop detections is often long, making it difficult to accurately correct the posture. Summary of the Invention

[0008] In order to solve the technical problems existing in the above-mentioned background technology, the present invention provides a laser positioning and mapping method and system based on slope factor, which effectively reduces positioning error and distortion of generated maps.

[0009] In order to achieve the above object, the present invention adopts the following technical solutions:

[0010] A first aspect of the present invention provides a laser positioning and mapping method based on a slope factor, comprising:

[0011] Acquire point cloud data and segment the point cloud data into ground point cloud and non-ground point cloud;

[0012] Based on the non-ground point cloud, perform point cloud registration, obtain the odometry factor and add it to the factor graph;

[0013] Based on the ground point cloud, slope estimation is performed to add the slope factor to the factor map;

[0014] Estimate the robot's pose based on the factor graph;

[0015] Based on the estimated pose, loop closure detection is performed to add loop closure factors to the factor graph;

[0016] Correct the pitch angle in the estimated pose;

[0017] Based on the corrected robot posture, a three-dimensional point cloud map is formed.

[0018] Furthermore, the point cloud data is segmented by a random sampling consensus algorithm.

[0019] Furthermore, the specific steps of the point cloud registration are:

[0020] Extract feature points from non-ground point clouds based on smoothness, and extract plane points and edge points;

[0021] For each frame of point cloud data, select several plane points and edge points with the highest smoothness as feature points;

[0022] Using the iterative closest point, the feature points are matched and the transfer matrix between two consecutive frames is obtained as the odometry factor.

[0023] Furthermore, the specific steps of the slope estimation are:

[0024] Set the detection area, select the ground point cloud in the detection area, and calculate the normal vector of the ground point cloud in the detection area;

[0025] According to the normal vector, the slope of the detection area and the robot plane is obtained.

[0026] Furthermore, the specific method of adding the slope factor is: setting the center point of the detection area at time i as the virtual target point; if at time i+△t, the robot moves to the virtual target point at time i, converting the slope of the detection area and the robot plane into a slope factor, and adding it to the posture nodes at time i and i+△t in the factor graph; wherein △t represents the time interval.

[0027] Furthermore, the specific method of the loop detection is: when the position of the robot at time j and the position of the robot at time i are less than a set value, and the slope at time j and the slope at time i are less than a threshold, the loop factor is added to the posture nodes at time i and j in the factor graph.

[0028] Furthermore, the specific method for correcting the pitch angle in the estimated posture is:

[0029] Obtain the robot's pitch angle through incremental estimation;

[0030] The pitch angle estimated by the incremental method and the pitch angle estimated based on the factor graph are combined to obtain the corrected pitch angle.

[0031] A second aspect of the present invention provides a laser positioning and mapping system based on a slope factor, comprising:

[0032] A point cloud segmentation module is configured to: acquire point cloud data, and segment the point cloud data into ground point cloud and non-ground point cloud;

[0033] A point cloud matching module is configured to: perform point cloud registration based on the non-ground point cloud, obtain the odometry factor and add it to the factor graph;

[0034] A slope estimation module is configured to: perform slope estimation based on the ground point cloud to add a slope factor to a factor map;

[0035] A laser odometry module is configured to: estimate the robot's pose based on a factor graph;

[0036] A loop closure detection module is configured to: perform loop closure detection based on the estimated pose to add a loop closure factor to the factor graph;

[0037] An incremental angle estimation module is configured to: correct the pitch angle in the estimated posture;

[0038] The laser mapping module is configured to form a three-dimensional point cloud map based on the corrected robot posture.

[0039] A third aspect of the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the above-mentioned laser positioning and mapping method based on a slope factor.

[0040] A fourth aspect of the present invention provides a computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps of the slope factor-based laser positioning and mapping method described above are implemented.

[0041] Compared with the prior art, the present invention has the following beneficial effects:

[0042] The present invention provides a laser positioning and mapping method based on a slope factor, which integrates the slope information in the environment into the SLAM algorithm and estimates the ground slope by processing ground laser point cloud data. The SLAM algorithm with the addition of this factor can effectively reduce positioning errors and distortion of the generated map.

[0043] The present invention provides a laser positioning and mapping method based on slope factor. The incremental angle estimation proposed by the method approximately estimates the pitch angle transformation of the robot by accumulating the slope of the ground, thereby realizing a real-time correction of the robot's pitch angle and improving the accuracy of the SLAM algorithm in pose estimation.

[0044] The present invention provides a laser positioning and mapping method based on a slope factor. This method uses loop detection including slope similarity analysis to determine whether a loop has occurred by comparing the similarity of terrain slopes when loops are generated. Compared with traditional methods, this method can reduce computing resources. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] The accompanying drawings, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0046] Figure 1 This is a flow chart of a laser positioning and mapping method based on a slope factor according to the first embodiment of the present invention;

[0047] Figure 2 Schematic diagram of slope estimation according to the first embodiment of the present invention;

[0048] Figure 3 This is a loop detection flow chart of the first embodiment of the present invention;

[0049] Figure 4 is a laser odometry factor graph according to the first embodiment of the present invention;

[0050] Figure 5 1 is a schematic diagram of incremental angle estimation according to the first embodiment of the present invention;

[0051] Figure 6 Schematic diagram of a real scene of laser mapping according to the first embodiment of the present invention;

[0052] Figure 7 Schematic diagram of the laser mapping result of the first embodiment of the present invention. DETAILED DESCRIPTION

[0053] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0054] It should be noted that the following detailed descriptions are illustrative and intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present invention belongs.

[0055] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is intended to include the plural form. In addition, it should be understood that when the terms "comprise" and / or "include" are used in this specification, they indicate the presence of features, steps, operations, devices, components and / or combinations thereof.

[0056] Example 1

[0057] This embodiment provides a laser positioning and mapping method based on slope factor, such as Figure 1 As shown, the specific steps include:

[0058] Step 1: Obtain point cloud data including the robot and the surrounding environment at a certain moment (current moment).

[0059] Specifically, point cloud data from the robot's lidar is obtained.

[0060] Step 2: Point cloud segmentation. Segment the point cloud data into ground point cloud and non-ground point cloud. Specifically, use the Random Sample Consensus (RANSAC) algorithm to segment the input point cloud into ground points and non-ground points to obtain the ground point cloud P. G and non-ground point clouds.

[0061] Before performing point cloud registration and slope estimation, the ground point cloud needs to be extracted. This extraction algorithm is based on the RANSAC algorithm and estimates the ground plane by setting a certain height threshold.

[0062] The RANSAC algorithm first randomly selects four points close to the ground, which can fit a plane. Then the plane continuously collects the points close to the plane and refits the entire plane. The points far away from the plane are considered as non-ground points P. C , after continuous iteration, the ground point cloud P can be obtained G .

[0063] Step 3: Point cloud matching: Use the non-ground point cloud to perform point cloud registration to obtain the robot pose transformation relationship (transfer matrix, i.e., odometry factor) between adjacent frames.

[0064] Point cloud matching is to obtain the robot pose change relationship between two consecutive frames based on the non-ground point clouds of two consecutive frames. Specifically:

[0065] Step 301: P C The points in the image are extracted based on their smoothness, and the plane points and edge points are extracted. The calculation method of the smoothness ζ of the point is as follows:

[0066]

[0067] Among them, p i Refers to the point where the smoothness is to be determined, including x, y and z coordinates; S is p i The neighborhood within the set range; p j is the neighborhood S except p i point.

[0068] When the smoothness of a point is greater than the set threshold, the point is considered to be a plane point; when it is less than the set threshold, it is considered to be an edge point.

[0069] Step 302: For each frame of point cloud data, select several plane points and edge points with the highest smoothness as feature points.

[0070] Specifically, for each frame of point cloud image, 80 plane points and 40 edge points with the highest smoothness are selected for the subsequent pose transfer solution.

[0071] Step 303: Use the Iterative Closest Point (ICP) to match the feature points, and finally obtain the transfer matrix between two consecutive frames as the odometry factor.

[0072] Step 4: Slope estimation: Use the points in the ground to detect the slope in real time.

[0073] Slope estimation mainly extracts the slope of the front area based on the ground point cloud obtained in step 2, such as Figure 2 As shown, the following steps are included:

[0074] Step 401: Set a detection area Γ in front of the robot. d , estimate the slope of the detection area;

[0075] Step 402: Select the ground point cloud P in the detection area s ,satisfy and

[0076] Step 403: Use the principal component analysis (PCA) algorithm to calculate the normal vector of the ground point cloud in the detection area.

[0077] Step 404: Calculate the horizontal angle between the robot coordinate system and the world coordinate system based on the normal vector obtained in step 403 to obtain the slope of the detection area and the robot plane.

[0078] Step 405: Set the center point of the detection area as the virtual target point. When the robot reaches the virtual target point, convert the slope information (the slope calculated in step 404) into a slope factor and add it to the factor graph framework as a constraint. Specifically, set the center point of the detection area at time i as the virtual target point. If the robot moves to the virtual target point at time i + Δt, convert the slope between the detection area and the robot plane into a slope factor and add it to the pose nodes at time i and time i + Δt in the factor graph, where Δt represents the time interval.

[0079] Step 5: Laser odometry: Use the Gtsam software library to build and update the factor graph and create a laser odometry.

[0080] Laser odometry primarily uses factor graphs to estimate the robot's pose. The factor graph, with the robot's pose as its node, consists of three different factors: odometry factor, slope factor, and loop factor. The factor graph uses graph optimization to continuously optimize the robot's pose, ultimately achieving an accurate pose.

[0081] In the laser data frames, keyframes are uniformly extracted based on the robot's travel distance and used to add robot pose nodes. This approach effectively conserves computing resources and enables the algorithm to run in real time. Each time a keyframe is added, a check is performed to determine whether a factor needs to be added and the addition is made accordingly.

[0082] The odometry factor is used to process the transfer matrix obtained from point cloud registration. The result is added to the factor graph as a factor between adjacent robot nodes. The slope factor incorporates the robot's angular transfer relationship. When the robot moves near the slope estimated at time i at time i+Δt, the slope factor is added to the robot nodes at time i+Δt and i as a constraint. When the robot moves to a previously reached position, the loopback factor is added between the node at the first reach of that position and the current node.

[0083] like Figure 4 As shown, x k Refers to the robot posture at the key frame moment during the robot's motion; i refers to the i-th key frame moment in the robot's operation; at time i+t, the robot reaches the position of the virtual target point generated at time i, and the slope factor is added; at time j, the robot moves to a position close to time i, and the loop factor is added.

[0084] The factor graph in the present invention includes a slope factor, which can use the slope information of the environment during the movement as a constraint condition to help the factor graph estimate and optimize the robot's posture.

[0085] Step 6: Loop closure detection: Loop closure detection monitors whether the robot revisits the area it has passed through to enrich the factor graph.

[0086] Loop closure detection determines whether the robot has reached its previous position by detecting the distance between the robot's current position and the previous trajectory, as well as the similarity between the current scene and the previous scene; it also takes into account the similarity of the scene ground slope. Specifically, when the position of the robot at time j is less than the set value compared to the position of the robot at time i, and the slope at time j is less than the threshold, the loop factor is added to the pose nodes at time i and j in the factor graph. Figure 3 As shown in Figure 2, the specific steps of loop detection include:

[0087] Step 601: Obtain the current robot posture obtained in step 5 (the posture includes information such as the robot's position, orientation, pitch angle, and yaw angle);

[0088] Step 602: Calculate the distance Δd between the current robot position and the historical trajectory;

[0089] Step 603: Determine whether Δd is less than a set value c. If so, extract the historical environment map, calculate the ground slope similarity, and proceed to step 604; otherwise, end the loop detection.

[0090] Step 604: Determine whether the ground slope similarity is less than a threshold value e. If so, match the historical scene and add a loop factor; otherwise, end the loop detection.

[0091] The slope similarity calculation formula is as follows:

[0092]

[0093] Among them, R i , R k Represents the rotation matrix of the robot's posture relative to the world coordinate system at the current moment and the historical moment respectively.

[0094] Step 7: Incremental Angle Estimation: The robot's pitch angle is obtained through incremental estimation. The pitch angle obtained through incremental estimation is combined with the pitch angle estimated based on the factor graph to obtain the corrected pitch angle.

[0095] Incremental angle estimation, such as Figure 5 As shown in the figure, the pitch angle of the robot at the current moment is estimated by recording and accumulating the changes in the slope factor during the robot's travel:

[0096]

[0097] Among them, φ i represents the pitch angle of the robot at time i, θ irepresents the detected slope value at time i. Here, since the robot pose at the initial moment is used as the origin of the world coordinate system, φ0 is equal to 0. Therefore, the change in the robot's pitch angle can be approximately equivalent to the accumulation of all the road slopes it has passed.

[0098] At this point, there are now two estimates of the robot's pitch angle. One is the pitch angle obtained by the laser odometry. One is the pitch angle φ obtained by incremental estimation i , and then combine the two according to formula (3) to obtain the processed pitch angle and output it.

[0099]

[0100] in, represents a preset threshold, and η is a weight coefficient that can be adjusted according to different environments. is the final output, the corrected pitch angle.

[0101] Incremental angle estimation makes real-time corrections to the robot's current state by accumulating the extracted slope information.

[0102] Step 8: Laser mapping.

[0103] Laser mapping is used to combine the keyframe point clouds extracted from the laser odometry to form a 3D point cloud map. Laser mapping obtains the pose of the keyframe point cloud map relative to the world based on the robot's pose relative to the world coordinate system at the time the keyframe is stored. The transferred keyframe point cloud map is then saved and gradually accumulated to obtain a point cloud map. The laser mapping result is as follows: Figure 6 and Figure 7 shown.

[0104] This example extracts the posture characteristics of a mobile robot when it encounters slopes in an environment. Furthermore, using a factor graph-based approach, a slope factor is proposed. This factor integrates slope characteristics into the factor graph to estimate the robot's pose. Furthermore, incremental angle estimation further improves SLAM performance, effectively reducing elevation errors and map distortion in laser SLAM, achieving better positioning performance.

[0105] Example 2

[0106] This embodiment provides a laser positioning and mapping system based on a slope factor, which specifically includes the following modules:

[0107] A point cloud segmentation module is configured to: acquire point cloud data including the robot and the surrounding environment, and segment the point cloud data into ground point cloud and non-ground point cloud;

[0108] A point cloud matching module is configured to: perform point cloud registration based on the non-ground point cloud, obtain the odometry factor and add it to the factor graph;

[0109] A slope estimation module is configured to: perform slope estimation based on the ground point cloud to add a slope factor to a factor map;

[0110] A laser odometry module is configured to: estimate the robot's pose based on a factor graph;

[0111] A loop closure detection module is configured to: perform loop closure detection based on the estimated pose to add a loop closure factor to the factor graph;

[0112] An incremental angle estimation module is configured to: correct the pitch angle in the estimated posture;

[0113] The laser mapping module is configured to form a three-dimensional point cloud map based on the corrected robot posture.

[0114] It should be noted here that the various modules in this embodiment correspond one-to-one to the various steps in Example 1, and the specific implementation processes are the same, which will not be repeated here.

[0115] Example 3

[0116] This embodiment provides a computer-readable storage medium having a computer program stored thereon. When the program is executed by a processor, the steps of the laser positioning and mapping method based on the slope factor as described in the first embodiment above are implemented.

[0117] Example 4

[0118] This embodiment provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, the steps of the slope factor-based laser positioning and mapping method described in the first embodiment are implemented.

[0119] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Thus, the present invention may take the form of hardware embodiments, software embodiments, or embodiments combining software and hardware. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to magnetic disk storage and optical storage, etc.) containing computer-usable program code.

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

[0121] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0122] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing 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.

[0123] Those skilled in the art will appreciate that all or part of the processes in the above-described method embodiments can be implemented by instructing related hardware through a computer program. The program can be stored in a computer-readable storage medium, and when executed, the program can include the processes in the above-described method embodiments. The storage medium can be a magnetic disk, an optical disk, a read-only memory (ROM), or a random access memory (RAM).

[0124] The foregoing description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Those skilled in the art will readily appreciate that various modifications and variations of the present invention are possible. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention are intended to be within the scope of protection of the present invention.

Claims

1. A laser positioning and mapping method based on slope factor, characterized in that: include: Acquire point cloud data and segment the point cloud data into ground point cloud and non-ground point cloud; Based on the non-ground point cloud, perform point cloud registration, obtain the odometry factor and add it to the factor graph; Based on the ground point cloud, slope estimation is performed to add the slope factor to the factor map; Estimate the robot's pose based on the factor graph; Based on the estimated pose, loop closure detection is performed to add loop closure factors to the factor graph; Correct the pitch angle in the estimated pose; Based on the corrected robot posture, a three-dimensional point cloud map is formed; The specific steps of the slope estimation are: setting a detection area, selecting a ground point cloud in the detection area, and calculating the normal vector of the ground point cloud in the detection area; according to the normal vector, obtaining the slope of the detection area and the robot plane; The specific method of adding the slope factor is: i The center point of the detection area at the moment is set as the virtual target point. i +△ t At time t, the robot moves to i The virtual target point at the moment, the slope of the detection area and the robot plane is converted into a slope factor, added to the factor graph i and i +△ t Among the pose nodes at the moment, △ t Indicates a time interval; The specific method of the loop detection is: j The robot's position at the moment i The robot's position at the moment is less than the set value, and j Time slope and i When the slope is less than the threshold, the loop factor is added to the factor graph. i and j In the pose node at the moment.

2. The laser positioning and mapping method based on slope factor according to claim 1, characterized in that: The point cloud data is segmented using a random sampling consensus algorithm.

3. The laser positioning and mapping method based on slope factor according to claim 1, characterized in that: The specific steps of point cloud registration are: Extract feature points from non-ground point clouds based on smoothness, and extract plane points and edge points; For each frame of point cloud data, select several plane points and edge points with the highest smoothness as feature points; Using the iterative closest point, the feature points are matched and the transfer matrix between two consecutive frames is obtained as the odometry factor.

4. The laser positioning and mapping method based on slope factor according to claim 1, wherein: The specific method for correcting the pitch angle in the estimated posture is: Obtain the robot's pitch angle through incremental estimation; The pitch angle estimated by the incremental method and the pitch angle estimated based on the factor graph are combined to obtain the corrected pitch angle.

5. A laser positioning and mapping system based on slope factor, characterized in that: include: A point cloud segmentation module is configured to: acquire point cloud data, and segment the point cloud data into ground point cloud and non-ground point cloud; A point cloud matching module is configured to: perform point cloud registration based on the non-ground point cloud, obtain the odometry factor and add it to the factor graph; A slope estimation module is configured to: perform slope estimation based on the ground point cloud to add a slope factor to a factor map; A laser odometry module is configured to: estimate the robot's pose based on a factor graph; A loop closure detection module is configured to: perform loop closure detection based on the estimated pose to add a loop closure factor to the factor graph; An incremental angle estimation module is configured to: correct the pitch angle in the estimated posture; a laser mapping module configured to: generate a three-dimensional point cloud map based on the corrected robot posture; The specific steps of the slope estimation are: setting a detection area, selecting a ground point cloud in the detection area, and calculating the normal vector of the ground point cloud in the detection area; according to the normal vector, obtaining the slope of the detection area and the robot plane; The specific method of adding the slope factor is: i The center point of the detection area at the moment is set as the virtual target point. i +△ t At time t, the robot moves to i The virtual target point at the moment, the slope of the detection area and the robot plane is converted into a slope factor, added to the factor graph i and i +△ t Among the pose nodes at the moment, △ t Indicates a time interval; The specific method of the loop detection is: j The robot's position at the moment i The robot's position at the moment is less than the set value, and j Time slope and i When the slope is less than the threshold, the loop factor is added to the factor graph. i and j In the pose node at the moment.

6. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the steps of the laser positioning and mapping method based on slope factor as described in any one of claims 1 to 4 are implemented.

7. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the program, the steps of the laser positioning and mapping method based on slope factor according to any one of claims 1 to 4 are implemented.

Images