3D laser SLAM method and mobile robot based on octogram and three-dimensional normal distribution optimization matching

By adopting an optimized matching method based on octave graph and three-dimensional normal distribution in the 3D laser SLAM method, the problem of inaccurate calculation of normal distribution parameters and large cumulative errors in the prior art is solved, and a higher precision positioning and Picture creation effect.

CN114202586BActive Publication Date: 2024-11-29YANGTZE RIVER DELTA HART ROBOT IND TECH RES INST
View PDF 2 Cites 0 Cited by

Patent Information

Application Number
CN202111521801.9
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2021-12-13
Publication Date
2024-11-29
Estimated Expiration
2041-12-13

AI Technical Summary

Technical Problem

When the existing 3D laser SLAM method matches laser frames and laser frames, voxel filtering leads to inaccurate calculation of normal distribution parameters, and matching laser frames based on laser frames will introduce large cumulative errors, affecting positioning accuracy.

Method used

Using the optimization matching method based on the octave graph and three-dimensional normal distribution, nonlinear optimization is performed through the octave graph voxel occupancy probability and three-dimensional normal distribution parameters, the current position of the 3D laser frame is obtained, and the voxel point set is updated. Three-dimensional normal distribution parameters and voxel occupancy probability.

Benefits of technology

It improves positioning accuracy, reduces cumulative errors, and enhances the absolute accuracy of environmental modeling, allowing the system to synchronize maps and position with higher efficiency, higher stability and accuracy.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN114202586B_ABST
    Figure CN114202586B_ABST
Patent Text Reader

Abstract

The present invention discloses a 3D laser SLAM method based on octogram and three-dimensional normal distribution optimization matching, comprising the following steps: S1, initializing the occupancy probability of voxels in the octogram based on the first 3D laser frame; S2, calculating the three-dimensional normal distribution parameters of the octogram voxel point set; S3, constructing the voxel occupancy probability and the three-dimensional normal distribution probability residual in the octogram based on the current laser frame, performing nonlinear optimization, and obtaining the current posture T of the 3D laser frame. t ; S4, based on the current posture T t Project the laser frame onto the octogram, insert the points in the laser frame into the voxel point set of the octogram, and update the three-dimensional normal distribution parameters and voxel occupancy probability of the voxel point set. The octogram voxel map stabilizes the absolute accuracy of SLAM to avoid large cumulative errors, and then the voxels are distributed in a three-dimensional normal manner so that the accuracy is not limited by the voxel size, and the repeated positioning accuracy is improved, so that the system can perform simultaneous mapping and positioning with higher efficiency, higher stability and accuracy.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the field of robot technology, and more specifically, the present invention relates to a 3D laser SLAM method and a mobile robot based on octogram and three-dimensional normal distribution optimization matching. Background Art

[0002] As mobile robots are increasingly used in unmanned driving and industrial warehousing and logistics, the SLAM technology for their positioning and navigation is becoming increasingly important. The existing technologies mainly include magnetic navigation, visual navigation, and laser SLAM navigation. Among them, the magnetic navigation working trajectory is fixed, and the path needs to lay electromagnetic wires or magnetic strips. It is generally used in small indoor scenes. The visual navigation trajectory is relatively free and the cost is low. Indoors, QR codes or special graphic codes and other markers are often used and require regular maintenance. Outdoor visual navigation is greatly affected by light sources and rainy and snowy environments, and has low reliability. Laser SLAM navigation has accurate positioning, flexible paths, and can adapt to a variety of on-site environments. After the breakthrough and maturity of laser radar technology, the cost has come down, and it has now become the mainstream positioning and navigation method for mobile robots.

[0003] The 3D laser SLAM algorithm builds a voxel map by matching point cloud and voxel map. It first performs voxel filtering point cloud preprocessing to reduce the point cloud density, and then directly calls the existing 3DNDT algorithm to calculate the motion pose increment by matching the front and back laser frames. After that, it uses this pose as the initial value and calls the ICP iterative closest point algorithm to optimize the pose increment. However, this solution has the following problems:

[0004] 1) The NDT algorithm called needs to calculate the ND parameters in the voxel. The calculation of the parameters depends on the number of points. Generally, NDT is used for laser frame and map matching, and a large number of points in the map are used to initialize the normal distribution. This solution uses laser frame and laser frame matching, and voxel filtering is performed in the first step to reduce a large number of points. Therefore, the calculated normal distribution parameters and position and posture increments are bound to be inaccurate.

[0005] 2) After using NDT matching to obtain the initial pose increment, ICP is used to optimize this increment. However, both NDT and ICP obtain pose increments based on matching laser frames to laser frames. These methods will have large cumulative errors, resulting in insufficient absolute accuracy of environment modeling, which in turn affects positioning accuracy. Summary of the invention

[0006] The present invention provides a 3D laser SLAM method based on octogram and three-dimensional normal distribution optimization matching, aiming at improving positioning accuracy.

[0007] The present invention is implemented as follows: a 3D laser SLAM method based on octogram and three-dimensional normal distribution optimization matching, the method comprising the following steps:

[0008] S1, initializing the occupancy probability of voxels in the octogram based on the first 3D laser frame;

[0009] S2, calculating the three-dimensional normal distribution parameters of the octogram voxel point set;

[0010] S3, based on the current laser frame, construct the voxel occupancy probability and the three-dimensional normal distribution probability residual in the octogram, perform nonlinear optimization, and obtain the current position T of the 3D laser frame t ;

[0011] S4, based on the current posture T t The laser frame is projected onto the octogram, the points in the laser frame are inserted into the voxel point set of the octogram, and the three-dimensional normal distribution parameters and voxel occupancy probability of the voxel point set are updated.

[0012] Furthermore, the current posture T t The specific method of obtaining is as follows:

[0013] An occupancy probability residual is constructed through the occupancy probability of the octet voxels, and then a second probability residual is constructed through the three-dimensional normal distribution function. The nonlinear optimization objective function F(T t ), as follows:

[0014]

[0015] Among them, w1 and w2 are the probability matching weight of the octogram and the probability matching weight of the three-dimensional normal distribution, w1+w2=1, T t-1 represents the world position at the last moment t-1, T t represents the world position at the current time t, represents the laser point p i The coordinates of the current moment t in the world coordinate system, represents the laser point p i The coordinates of the previous moment t-1 in the world coordinate system, E represents the mean of all points in the voxel in the octet, that is, the expected value;

[0016] The objective function is iteratively optimized by the classic Gauss-Newton method to obtain the world pose T at time t t .

[0017] Furthermore, the occupancy probability function M(p i )The calculation formula is as follows:

[0018]

[0019] Among them, G(p i ) represents the laser point p iThe integer coordinates of the corresponding voxels in the octogram, logodds(G(p i )) represents the laser point p i The occupancy win rate value of the corresponding voxel in the octet graph, M(p i ) represents the laser point p i The occupancy probability of the corresponding voxel in the octogram.

[0020] The present invention is implemented as follows: a mobile robot includes a processor and a memory, wherein the memory stores at least one instruction, at least one program, a code set or an instruction set, and the at least one instruction, the at least one program, the code set or the instruction set are loaded and executed by the processor to implement the 3D laser SLAM method based on octogram and three-dimensional normal distribution optimization matching as described above.

[0021] This technical solution proposes a 3D laser SLAM method based on octogram and three-dimensional normal distribution optimization matching. The octogram voxel map is used to stabilize the absolute accuracy of SLAM and avoid large cumulative errors. The voxels are then normalized in three dimensions so that the accuracy is not limited by the voxel size, thereby improving the accuracy of repeated positioning and enabling the system to perform simultaneous mapping and positioning with higher efficiency, stability and accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 A flow chart of a 3D laser SLAM method based on octogram and three-dimensional normal distribution optimization matching provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0023] The specific implementation modes of the present invention are further explained in detail below by describing the embodiments with reference to the accompanying drawings, so as to help those skilled in the art to have a more complete, accurate and in-depth understanding of the inventive concept and technical solution of the present invention.

[0024] Figure 1 A flow chart of a 3D laser SLAM method based on octogram and three-dimensional normal distribution optimization matching provided in an embodiment of the present invention, the method specifically comprises the following steps:

[0025] (1) Initialize the octogram and the occupancy probability of the voxels in the octogram based on the first 3D laser frame;

[0026] The midpoint p of the 3D laser frame is known i Coordinates in the world coordinate system Calculate the point p through the function G i Voxel indices (indx, indy, indz) in octograms of different resolutions resi Here, the function G is mainly divided by different resolutions, and then rounded to get the voxel index, that is, G(pi ) represents point p i Integer coordinates in an octogram The known points in the octogram are voxels, and the logodds probability of the corresponding voxels is initialized to 0. Calculate the occupancy probability value M(p i ) needs to convert its occupation win rate value logodds probability into occupation probability. The occupation probability function M(p i )The calculation formula is as follows:

[0027]

[0028] Among them, G(p i ) represents the laser point p i The integer coordinates of the corresponding voxels in the octogram, logodds(G(p i )) represents the laser point p i The occupancy win rate value of the corresponding voxel in the octet graph, M(p i ) represents the laser point p i The occupancy probability of the corresponding voxel in the octogram.

[0029] (2) Calculate the three-dimensional normal distribution parameters of the octogram voxel point set;

[0030] Given the point set in the octogram voxel, when the number of internal points exceeds 3, calculate its three-dimensional normal distribution parameters ND(E, Cov, f(p)), where the parameters E, Cov, and f(p) represent the expectation, variance matrix, and three-dimensional normal distribution probability function, respectively. i The three-dimensional normal distribution probability function f(p i )as follows:

[0031]

[0032]

[0033]

[0034] Where n is the number of voxels in the octogram, represents voxel p i Floating point coordinates in the octogram.

[0035] (3) Based on the current laser frame, the voxel occupancy probability and the three-dimensional normal distribution probability residual in the octogram are constructed, and the current position and posture of the laser frame are obtained after nonlinear optimization;

[0036] Project the current laser frame into the octogram map and calculate the voxel coordinates of each laser point An occupancy probability residual is constructed through the occupancy probability of the octet voxels, and then a second probability residual is constructed through the three-dimensional normal distribution function. The nonlinear optimization objective function F(T t ), as follows:

[0037]

[0038] Where: w1 and w2 are the probability matching weight of the octogram and the probability matching weight of the three-dimensional normal distribution, w1+w2=1, T t-1 represents the world position at the last moment t-1, T t represents the world position at the current time t, represents the laser point p i The coordinates of the current moment t in the world coordinate system, represents the laser point p i The coordinates of the previous moment t-1 in the world coordinate system, E represents the mean of all points in the voxel in the octogram, that is, the expected value.

[0039] Known world position T at the last moment t-1 , and using this as the initial value, the objective function is iteratively optimized through the classic Gauss-Newton method to obtain the world posture T at time t t , to achieve map-based positioning in slam.

[0040] (4) Based on the current posture, the laser frame is projected onto the octogram, and the voxel point set, voxel probability, and three-dimensional normal distribution parameters are updated, waiting for the next matching

[0041] Through the pose T t The current laser frame is projected onto the octogram, and the points in the laser frame are inserted into the corresponding voxel point set, and the three-dimensional normal distribution parameters ND(E, Cov, f(p)) of the octogram are recalculated.

[0042] The voxel occupancy win rate value of the octogram is represented by logodds(G(p)). After the voxel is initialized, the logodds value is 0. The updated occupation probability pocc∈(0.5,1.0) and the free probability pfree∈(0,0.5) are set. Then the updated logodds is as follows:

[0043] When the occupation probability pocc∈(0.5,1.0), logodds(G(p)) is calculated based on the following formula (6):

[0044]

[0045] When the free probability pfree∈(0,0.5), the logodds(G(p)) is calculated based on the following formula (7):

[0046]

[0047] The updated voxel logodds can be used to solve the voxel occupancy probability M of the octogram coordinates through formula (1), thus obtaining the updated octogram, in which each voxel contains the three-dimensional normal parameter ND(E, Cov, f(p)) and the voxel occupancy probability M. The above process is the mapping process.

[0048] An embodiment of the present application also provides a mobile robot, comprising: a processor and a memory, wherein the memory stores at least one instruction, at least one program, a code set or an instruction set, and the at least one instruction, the at least one program, the code set or the instruction set are loaded and executed by the processor to implement the above-mentioned 3D laser SLAM method based on octogram and three-dimensional normal distribution optimization matching.

[0049] The present invention is described above by way of example in conjunction with the accompanying drawings. It is obvious that the specific implementation of the present invention is not limited to the above-mentioned method. As long as various non-substantial improvements are made using the method concept and technical solution of the present invention, or the concept and technical solution of the present invention are directly applied to other occasions without improvement, they are all within the protection scope of the present invention.

Claims

1. A 3D laser SLAM method based on octogram and three-dimensional normal distribution optimization matching, characterized in that: The method comprises the following steps: S1, initializing the occupancy probability of voxels in the octogram based on the first 3D laser frame; S2, calculating the three-dimensional normal distribution parameters of the octogram voxel point set; S3: Based on the current laser frame, the voxel occupancy probability and the three-dimensional normal distribution probability residual in the octogram are constructed, and nonlinear optimization is performed to obtain the current position T of the 3D laser frame. t ; S4, based on the current posture T t Projecting the laser frame onto the octogram, inserting the points in the laser frame into the voxel point set of the octogram, and updating the three-dimensional normal distribution parameters and voxel occupancy probability of the voxel point set; Current pose T t The specific method of obtaining is as follows: An occupancy probability residual is constructed through the occupancy probability of the octet voxels, and then a second probability residual is constructed through the three-dimensional normal distribution function. The nonlinear optimization objective function F(T t ), as follows: Among them, w1 and w2 are the probability matching weight of the octogram and the probability matching weight of the three-dimensional normal distribution, w1+w2=1, T t-1 represents the world position at the last moment t-1, T t represents the world position at the current time t, represents the laser point p i The coordinates of the current moment t in the world coordinate system, represents the laser point p i The coordinates of the previous moment t-1 in the world coordinate system, E represents the mean of all points in the voxel in the octet, that is, the expected value; The objective function is iteratively optimized by the classic Gauss-Newton method to obtain the world pose T at time t t .

2. The 3D laser SLAM method based on octogram and three-dimensional normal distribution optimization matching as claimed in claim 1, characterized in that: The occupancy probability function M(p i )The calculation formula is as follows: Among them, G(p i ) represents the laser point p i The integer coordinates of the corresponding voxels in the octogram, logodds(G(p i )) represents the laser point p i The occupancy win rate value of the corresponding voxel in the octet graph, M(p i ) represents the laser point p i The occupancy probability of the corresponding voxel in the octogram.

3. A mobile robot, characterized in that: The mobile robot includes a processor and a memory, wherein the memory stores at least one instruction, at least one program, a code set or an instruction set, and the at least one instruction, the at least one program, the code set or the instruction set are loaded and executed by the processor to implement the 3D laser SLAM method based on octogram and three-dimensional normal distribution optimization matching as described in any one of claims 1-2.

Citation Information

Patent Citations

  • Three-dimensional laser radar positioning and navigation method for intelligent inspection and inspection robot

    CN112014857A

  • Techniques for real-time mapping in a movable object environment

    CN112955713A