Laser odometer method fusing range constraint

Abstract

The invention relates to the technical field of SLAM (Simultaneous Localization and Mapping), in particular to a range constraint fused laser odometer method. Comprising the following steps: step 1, initializing a laser radar; 2, acquiring initial point cloud data; and step 3, constraining the fusion range of the laser speedometer. According to the method, the initial pose of the point cloud is obtained through the PL-ICP algorithm, laser scanning range constraints are fused, a unified least square objective function is constructed, the optimal pose of the mobile robot is solved, the PL-ICP algorithm takes the distance from the point to the line as an error, and the accurate initial pose of the robot is obtained under the condition that the movement speed is high; a unified least square objective function is constructed by fusing laser scanning range constraints, namely, errors between maps generated by a current frame and all previous frames are considered, and matching errors between adjacent frames are reflected, so that the influence of accidental objects or moving objects on a matching result can be effectively removed, and the matching stability and precision are improved.

Description

Technical Field

[0001] The invention relates to the field of SLAM technology, in particular to a laser odometer method integrating range constraints. Background Art

[0002] Currently, mobile robots are increasingly used in warehousing, services, and logistics, and are also expanding into manufacturing, medical care, agriculture and other fields. Their autonomous positioning and navigation technology is becoming increasingly important. SLAM technology has developed rapidly in recent years and has been widely used. Whether in unmanned transportation, autonomous driving, or service robots, SLAM technology covers most scenarios from high-speed to low-speed motion, and the application scenarios also range from limited indoor environments to vast outdoor environments. LiDAR positioning is a basic task that needs to be completed in SLAM motion estimation, and continuous positioning results realize the function of the odometer.

[0003] The process of laser SLAM can usually be divided into front-end matching and back-end optimization stages. Back-end optimization mainly involves graph optimization steps. Its basic principle is to improve the overall accuracy of the system by solving the least squares solution. In order to achieve high-precision laser SLAM, the accuracy of the front-end registration stage is crucial. The existing laser SLAM adopts the solution of matching the laser point cloud at the current moment with the point cloud of the previous frame to achieve positioning. There are the following problems:

[0004] Robots often use motion models to estimate their own position changes between two time points. If the motion model is inaccurate, each estimation will introduce errors, and the accumulation of errors will cause position drift. If there are errors in the previous matching and these errors are not corrected in subsequent estimates, the errors will be passed on to subsequent estimates, resulting in cumulative errors. Over time, the estimation errors caused by the above reasons will gradually accumulate, causing the robot positioning drift. Summary of the invention

[0005] To solve the above problems, the present invention provides a laser odometer method integrating range constraints, which can reduce errors and improve the accuracy of the laser odometer and the positioning capability of the SLAM front-end.

[0006] The technical solution of the present invention is described as follows in conjunction with the accompanying drawings:

[0007] A laser odometer method integrating range constraints comprises the following steps:

[0008] Step 1: Initialize the laser radar;

[0009] Step 2: Obtain initial point cloud data;

[0010] Step 3: Laser odometry fusion range constraints.

[0011] Furthermore, the specific method of step one is as follows:

[0012] 11) Start the LiDAR and ensure that the equipment is in normal working condition, including checking the sensor connection, power supply, and the status of the laser transmission and receiving systems;

[0013] 12) According to the technical specifications of the LiDAR, configure relevant parameters to ensure that the LiDAR can provide point cloud data with appropriate density and accuracy;

[0014] 13) Perform external parameter calibration to determine the installation position and direction of the laser radar relative to the robot coordinate system.

[0015] Furthermore, the specific method of step 2 is as follows:

[0016] 21) At the initial position of the robot, start the laser radar device to scan and generate point cloud data;

[0017] 22) Preprocess the original point cloud.

[0018] Furthermore, the specific method of step three is as follows:

[0019] 31) Calculate the robot's initial pose, point cloud matching residual and range constraint residual between adjacent frames;

[0020] 32) Perform nonlinear optimization on the point cloud matching residual and the range constraint residual to obtain the optimal pose at time T;

[0021] 33) Project the laser frame at time T to the optimal position of the grid map, update the grid map, and continuously repeat the above steps to realize the function of the laser odometer.

[0022] Further, the specific method of step 31) is as follows:

[0023] 311) The calculation method of the robot's initial posture between adjacent frames is as follows:

[0024] The laser frame at time T is taken as the current frame, and the laser frame at time T-1 is taken as the reference frame;

[0025] Project the point cloud of the current frame onto the coordinate system of the reference frame, and use the nearest neighbor rule to find each point p in the current frame. i (x i ,y i ), the two nearest points in the reference frame

[0026] Get the straight line Ax+By+C=0 where the two closest points are located, and calculate the distance from point pi to the straight line where the two closest points are located. The distance value represents the error value, as shown below:

[0027]

[0028] In the formula, For point p i (x i ,y i ) is the vector pointing to the point projected onto the line;

[0029] Put all the points in the current frame into a point set, traverse all the points in the point set according to the distance from the point to the line, delete the points that exceed the specified threshold, and obtain the initial posture of the robot by optimizing the objective function; the objective function is as follows:

[0030]

[0031] Where ni is the normal of the surface at the projection point; the symbol is the rotation and translation operator; Π{S ref ,·} is the reference surface S ref Euclidean projection on ;

[0032] By using the least squares method to optimize the objective function E, the optimal transformation matrix is ​​obtained as the initial position P of the radar from time (T-1) to time T 0 ;

[0033] 312) Calculate the point cloud matching residual term ζ of the current frame relative to the reference frame, expressed as follows:

[0034]

[0035] 313) The range constraint residual is calculated as follows:

[0036] At time T-1, the following is observed at the scanning coordinate μ, which is directly related to the angular coordinate of a laser point P of the scanned object:

[0037]

[0038] Where RAN is the scanning field of view of one frame of the lidar. The linear constraint is derived from the general expression of the geometric consistency of two scan pairs. W is differentiable. The range of any point scanned at time T is expressed as a Taylor expansion as follows:

[0039]

[0040] Where Δt is the time interval between consecutive scans; Δμ is the change in the scan coordinates of the point under consideration. Ignoring higher-order terms, the scan gradient is related to the range of the point and the change in the scan coordinates during Δt:

[0041]

[0042] In the formula, ΔW+W(t,μ)=W(t+Δt,μ+Δμ),

[0043] and is the average velocity of the points and scanning coordinates within the range of Δt, and we get:

[0044]

[0045] The radial and azimuthal velocities In Cartesian space:

[0046]

[0047] Imposing a rigidity assumption:

[0048] Convert the range constraint equation to a constraint on the lidar velocity:

[0049]

[0050] Due to the noise in the distance measurement, the error caused by the linear approximation or the presence of moving objects, for each point, the range constraint residual ρ is expressed as follows:

[0051]

[0052] Further, the specific method of step 32) is as follows:

[0053] 321) The least squares objective function F is constructed based on the point cloud matching residual and the range constraint residual, which is expressed as follows:

[0054]

[0055] In the formula, λ 1 and λ 2 are the weight values ​​of the point cloud matching residual term and the range constraint residual term respectively;

[0056] 322) Based on the Levenberg-Marquardt optimization method, the least squares objective function F is optimized to obtain the optimal posture P at time T T , as shown below:

[0057]

[0058] Where P 0 is the initial pose matrix; J P is the derivative of the objective function with respect to the pose; ε is the penalty factor.

[0059] Further, the specific method of step 33) is as follows:

[0060] Project the laser frame at time T to the optimal pose P of the grid map T At the location, update the grid map, and continuously repeat the above steps to realize the function of the laser odometer.

[0061] The beneficial effects of the present invention are:

[0062] 1) The present invention can reduce errors and improve the accuracy of laser odometers and the positioning capability of SLAM front-ends;

[0063] 2) The present invention obtains the initial pose of the point cloud through the PL-ICP algorithm, integrates the laser scanning range constraints, constructs a unified least squares objective function, and solves the optimal pose of the mobile robot. The PL-ICP algorithm uses the distance from the point to the line as the error. Under the condition of fast movement speed, a more accurate initial pose of the robot is obtained; the unified least squares objective function is constructed by integrating the laser scanning range constraints, that is, the error between the current frame and the map generated by all previous frames is considered, and the matching error between adjacent frames is reflected. It can effectively remove the influence of unexpected objects or moving objects on the matching results and improve the stability and accuracy of matching. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0065] Figure 1 A schematic diagram of a flow chart of a laser odometer method integrating range constraints according to the present invention;

[0066] Figure 2 It is a schematic diagram of the trajectory comparison between the PL-ICP method and the present invention;

[0067] Figure 3 Schematic diagram of the error between the PL-ICP method and the true trajectory;

[0068] Figure 4 Schematic diagram of absolute trajectory error of PL-ICP method;

[0069] Figure 5 A schematic diagram of the error between the present invention and the actual trajectory;

[0070] Figure 6 Schematic diagram of the absolute trajectory error of the present invention. DETAILED DESCRIPTION

[0071] The present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It is to be understood that the specific embodiments described herein are only used to explain the present invention, rather than to limit the present invention. It should also be noted that, for ease of description, only parts related to the present invention, rather than all structures, are shown in the accompanying drawings.

[0072] Embodiment 1

[0073] This embodiment provides a laser odometer method integrating range constraints, including the following steps:

[0074] Step 1: Initialize the laser radar, as follows:

[0075] 11) Start the LiDAR and ensure that the equipment is in normal working condition, including checking the sensor connection, power supply, and the status of the laser transmission and receiving systems;

[0076] 12) According to the technical specifications of the LiDAR, configure relevant parameters, such as scanning frequency, angle range, and angle resolution, to ensure that the LiDAR can provide point cloud data with appropriate density and accuracy;

[0077] 13) Perform external parameter calibration to determine the installation position and direction of the laser radar relative to the robot coordinate system.

[0078] Step 2: Get the initial point cloud data, as follows:

[0079] 21) At the initial position of the robot, start the laser radar device to scan and generate point cloud data;

[0080] 22) Preprocess the original point cloud, such as denoising and downsampling, to reduce unnecessary computational burden and improve matching efficiency.

[0081] Steps 1 and 2 are the preparation part, which ensures that the method is not interfered by the sensor itself and external factors during implementation, and ensures the accuracy of subsequent results;

[0082] Step 3: Laser odometer fusion range constraints, as follows:

[0083] 31) Calculate the robot's initial pose, point cloud matching residual and range constraint residual between adjacent frames, as follows:

[0084] 311) The calculation method of the robot's initial posture between adjacent frames is as follows:

[0085] The laser frame at time T is taken as the current frame, and the laser frame at time T-1 is taken as the reference frame;

[0086] Project the point cloud of the current frame onto the coordinate system of the reference frame, and use the nearest neighbor rule to find each point p in the current frame.i (x i ,y i ), the two nearest points in the reference frame

[0087] Get the straight line Ax+By+C=0 where the two closest points are located, and calculate the distance from point pi to the straight line where the two closest points are located. The distance value represents the error value, as shown below:

[0088]

[0089] In the formula, For point p i (x i ,y i ) is the vector pointing to the point projected onto the line;

[0090] Put all the points in the current frame into a point set, traverse all the points in the point set according to the distance from the point to the line, delete the points that exceed the specified threshold, and obtain the initial posture of the robot by optimizing the objective function; the objective function is as follows:

[0091]

[0092] Where ni is the normal of the surface at the projection point; the symbol is the rotation and translation operator; Π{S ref ,·} is the reference surface S ref Euclidean projection on ;

[0093] By using the least squares method to optimize the objective function E, the optimal transformation matrix is ​​obtained as the initial position P of the radar from time (T-1) to time T 0 ;

[0094] 312) Calculate the point cloud matching residual term ζ of the current frame relative to the reference frame, expressed as follows:

[0095]

[0096] 313) The range constraint residual is calculated as follows:

[0097] At time T-1, the following is observed at the scanning coordinate μ, which is directly related to the angular coordinate of a laser point P of the scanned object:

[0098]

[0099] Where RAN is the scanning field of view of one frame of the lidar. The linear constraint is derived from the general expression of the geometric consistency of two scan pairs. W is differentiable. The range of any point scanned at time T is expressed as a Taylor expansion as follows:

[0100]

[0101] Where Δt is the time interval between consecutive scans; Δμ is the change in the scan coordinates of the point under consideration. Ignoring higher-order terms, the scan gradient is related to the range of the point and the change in the scan coordinates during Δt:

[0102]

[0103] In the formula, ΔW+W(t,μ)=W(t+Δt,μ+Δμ),

[0104] and is the average velocity of the points and scanning coordinates within the range of Δt, and we get:

[0105]

[0106] The radial and azimuthal velocities In Cartesian space:

[0107]

[0108] Imposing a rigidity assumption:

[0109] Convert the range constraint equation to a constraint on the lidar velocity:

[0110]

[0111] Due to the noise in the distance measurement, the error caused by the linear approximation or the presence of moving objects, for each point, the range constraint residual ρ is expressed as follows:

[0112]

[0113] 32) The point cloud matching residual and the range constraint residual are jointly optimized nonlinearly to obtain the optimal pose at time T, as follows:

[0114] 321) The least squares objective function F is constructed based on the point cloud matching residual and the range constraint residual, which is expressed as follows:

[0115]

[0116] In the formula, λ 1 and λ 2 are the weight values ​​of the point cloud matching residual term and the range constraint residual term respectively;

[0117] 322) Based on the Levenberg-Marquardt optimization method, the least squares objective function F is optimized to obtain the optimal posture P at time T T , as shown below:

[0118]

[0119] Where P 0 is the initial pose matrix; J P is the derivative of the objective function with respect to the pose; ε is the penalty factor.

[0120] 33) Project the laser frame at time T to the optimal position of the grid map, update the grid map, and continuously repeat the above steps to realize the function of the laser odometer, as follows:

[0121] Project the laser frame at time T to the optimal pose P of the grid map T At the location, update the grid map, and continuously repeat the cycle of step 2-step 3 to realize the function of the laser odometer.

[0122] In summary, the fusion of laser scanning range constraints helps to improve the estimation accuracy of posture and position, consider the range information of laser data, better fit the robot's motion model, thereby reducing the error of posture estimation and the uncertainty caused by environmental noise or sensor error. The algorithm is more capable of robustly handling the noise in sensor data and better adapting to complex environments. The fusion of laser scanning range constraints makes the robot navigation and positioning tasks more robust and adaptable, and improves the accuracy and efficiency of posture estimation.

[0123] Embodiment 2

[0124] In order to verify the effectiveness and superiority of the method proposed in the invention, the existing method PL-ICP method was selected to compare and verify with the present invention;

[0125] like Figure 2-Figure 6 As shown, the average error of the PL-ICP method is 0.158034, the standard deviation is 0.057917, the average error of the method of the present invention is 0.087754, the standard deviation is 0.025027, and the error is reduced by 55.52%;

[0126] It can be seen from this that the present invention can reduce errors and improve the accuracy of the laser odometer and the positioning capability of the SLAM front end.

[0127] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A laser odometer method integrating range constraints, characterized in that: The following steps are involved: Step 1: Initialize the laser radar; Step 2: Obtain initial point cloud data; Step 3: Laser odometry fusion range constraints.

2. A laser odometer method integrating range constraints according to claim 1, characterized in that: The specific method of step one is as follows: 11) Start the LiDAR and ensure that the equipment is in normal working condition, including checking the sensor connection, power supply, and the status of the laser transmission and receiving systems; 12) According to the technical specifications of the LiDAR, configure relevant parameters to ensure that the LiDAR can provide point cloud data with appropriate density and accuracy; 13) Perform external parameter calibration to determine the installation position and direction of the laser radar relative to the robot coordinate system.

3. The laser odometer method integrating range constraints according to claim 1, characterized in that: The specific method of step 2 is as follows: 21) At the initial position of the robot, start the laser radar device to scan and generate point cloud data; 22) Preprocess the original point cloud.

4. The laser odometer method integrating range constraints according to claim 1, characterized in that: The specific method of step three is as follows: 31) Calculate the robot's initial pose, point cloud matching residual and range constraint residual between adjacent frames; 32) Perform nonlinear optimization on the point cloud matching residual and the range constraint residual to obtain the optimal pose at time T; 33) Project the laser frame at time T to the optimal position of the grid map, update the grid map, and continuously repeat the above steps to realize the function of the laser odometer.

5. The laser odometer method integrating range constraints according to claim 1, characterized in that: The specific method of step 31) is as follows: 311) The calculation method of the robot's initial posture between adjacent frames is as follows: The laser frame at time T is taken as the current frame, and the laser frame at time T-1 is taken as the reference frame; Project the point cloud of the current frame onto the coordinate system of the reference frame, and use the nearest neighbor rule to find each point p in the current frame. i (x i ,y i ), the two nearest points in the reference frame Get the straight line Ax+By+C=0 where the two closest points are located, and calculate the distance from point pi to the straight line where the two closest points are located. The distance value represents the error value, as shown below: In the formula, For point p i (x i ,y i ) is the vector pointing to the point projected onto the line; Put all the points in the current frame into a point set, traverse all the points in the point set according to the distance from the point to the line, delete the points that exceed the specified threshold, and obtain the initial posture of the robot by optimizing the objective function; the objective function is as follows: Where n i is the normal of the surface at the projection point; the symbol is the rotation and translation operator; Π{S ref ,·} is the reference surface S ref Euclidean projection on ; By using the least squares method to optimize the objective function E, the optimal transformation matrix is ​​obtained as the initial position P0 of the radar from time (T-1) to time T; 312) Calculate the point cloud matching residual term ζ of the current frame relative to the reference frame, expressed as follows: 313) The range constraint residual is calculated as follows: At time T-1, the following is observed at the scanning coordinate μ, which is directly related to the angular coordinate of a laser point P of the scanned object: Where RAN is the scanning field of view of one frame of the lidar. The linear constraint is derived from the general expression of the geometric consistency of two scan pairs. W is differentiable. The range of any point scanned at time T is expressed as a Taylor expansion as follows: Where Δt is the time interval between consecutive scans; Δμ is the change in the scan coordinates of the point under consideration. Ignoring higher-order terms, the scan gradient is related to the range of the point and the change in the scan coordinates during Δt: In the formula, ΔW+W(t,μ)=W(t+Δt,μ+Δμ), and is the average velocity of the points and scanning coordinates within the range of Δt, and we get: The radial and azimuthal velocities In Cartesian space: Imposing a rigidity assumption: Convert the range constraint equation to a constraint on the lidar velocity: Due to the noise in the distance measurement, the error caused by the linear approximation or the presence of moving objects, for each point, the range constraint residual ρ is expressed as follows:

6. The laser odometer method integrating range constraints according to claim 1, characterized in that: The specific method of step 32) is as follows: 321) The least squares objective function F is constructed based on the point cloud matching residual and the range constraint residual, which is expressed as follows: Where λ1 and λ2 are the weight values ​​of the point cloud matching residual term and the range constraint residual term respectively; 322) Based on the Levenberg-Marquardt optimization method, the least squares objective function F is optimized to obtain the optimal posture P at time T T , as shown below: Where P0 is the initial pose matrix; J P is the derivative of the objective function with respect to the pose; ε is the penalty factor.

7. The laser odometer method integrating range constraints according to claim 1, characterized in that: The specific method of step 33) is as follows: Project the laser frame at time T to the optimal pose P of the grid map T At the location, update the grid map, and continuously repeat the above steps to realize the function of the laser odometer.

Images