Fast laser SLAM algorithm based on intensity information matching of quadruped robot
By combining laser intensity information in the quadruped robot laser SLAM algorithm for feature point extraction and pose estimation, the problems of low computing efficiency and low positioning accuracy in the existing technology are solved, and more efficient feature point matching and real-time construction are achieved. Picture effect.
Patent Information
- Application Number
- CN202210339011.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-04-01
- Publication Date
- 2024-11-26
- Estimated Expiration
- 2042-04-01
AI Technical Summary
The existing four-legged robot laser SLAM algorithm only considers geometric information during feature point extraction process and ignores laser intensity information, resulting in low computing efficiency, low positioning accuracy, and difficult to realize real-time map construction on platforms with limited resources.
During the process of feature point extraction and frame map matching, laser intensity information is used as an important feature and combined with geometric information, filtering, dimensionality reduction, feature extraction and pose estimation of point cloud data, and distortion compensation is performed after pose estimation. , update the undistorted feature points into the map, improve positioning accuracy and reduce the calculation amount.
By introducing laser intensity information, the accuracy of feature point extraction and frame map matching efficiency are improved, the real-time map construction capability of the four-legged robot under limited resources is enhanced, and the positioning accuracy of the same location is improved.
Smart Images

Figure QLYQS_1 
Figure QLYQS_2 
Figure QLYQS_3
Abstract
Description
Technical Field
[0001] The invention relates to the field of quadruped robot control, in particular to a fast laser SLAM algorithm based on intensity information matching of a quadruped robot. Background Art
[0002] The current laser SLAM (Simultaneous Location and Mapping) algorithm mainly uses LOAM as the main framework, and then improves on it. In the process of sensor data acquisition in the LOAM algorithm, the ground conditions will cause sensor noise to exist all the time. For example, the data returned by the sensor from the grass may be very rough, and the edge feature extraction will be unreliable. Therefore, unreliable data needs to be eliminated before feature point extraction; in the feature point extraction process, only geometric information is used, while the intensity information emitted and returned by the laser is not used; in the frame image matching process, it takes a lot of time to calculate the matching information, and the calculation efficiency is low. In the current scenario, the computing resources on the robot platform are limited, so the optimization algorithm is particularly important for the calculation method of frame image matching. Based on the above analysis, LOAM's laser SLAM algorithm can no longer meet the needs of map construction. The laser SLAM algorithm based on intensity information removes noise before extracting feature points to improve the accuracy of feature point extraction. In the frame matching and motion estimation process, the distortion calculation will be performed again after the pose estimation processing. After the calculation, the undistorted features are updated in the map, which improves the positioning accuracy of the same location and reduces the amount of calculation.
[0003] For the laser SLAM algorithm based on intensity information, in the current laser SLAM algorithm, real-time processing involves the matching of point clouds. The existing matching can be divided into two types, one is the matching of the original point cloud, and the other is the matching of feature points. The common algorithm for the original point cloud matching is ICP (Iterative Closest Points), which calculates the corresponding points by finding the nearest points in the Euclidean space, and then iterates the distance residual between the corresponding points to minimize it, and the posture transformation in the two point clouds converges to the final position. Another point cloud matching algorithm is IMLS-SLAM, which assigns weights to each point, and each weight value is calculated by IMLS (Implicit Least Square) based on the local surface normal of the target point. However, the computational consumption of the original point cloud matching is too high. Feature point matching is introduced in LOAM to match the point cloud. The main point cloud matching method used is to extract plane points and edge points as feature points, and distinguish edge points and plane points by calculating the smoothness of the local point cloud. However, during the laser emission and return process, the laser intensity information was not taken into account, and was only analyzed at the geometric level, resulting in low information utilization.
[0004] The feature point extraction of the LOAM algorithm only considers geometric features, such as edge points and plane points, and the power also changes during the laser emission and return process. In the frame-map matching process, the calculation time is long and the mapping effect is poor. The present invention adds the characteristics of laser intensity while extracting the eigenvalues, which is more conducive to the accuracy of feature point extraction. At the same time, due to the limited resources on the quadruped robot platform, complex algorithm calculations cannot be performed, and the mapping time cannot guarantee real-time effects. Therefore, it is particularly important to simplify the algorithm complexity while ensuring accuracy as much as possible. The present invention takes into account that existing laser radars all run at more than 10Hz and the scanning time of each frame is very short. Therefore, it can be assumed that the angular velocity and linear velocity in a short time are fixed, thereby predicting motion and correcting distortion. After the pose estimation, the distortion is calculated again, and the undistorted features are updated to the map, which can improve the positioning accuracy of the same location and reduce the amount of calculation. Summary of the invention
[0005] In order to solve the above technical problems, the present invention proposes a fast laser SLAM algorithm based on intensity information matching of a quadruped robot. The laser SLAM algorithm involved in the present invention has made improvements in the information utilization and calculation consumption of feature point extraction. In the feature extraction method involved, in addition to utilizing geometric information, it is proposed to use the intensity of the laser as important information in feature matching; at the same time, the motion estimation module has been improved. After obtaining the posture estimation, the distortion is calculated again, and the undistorted features after calculation are updated to the map to improve the positioning accuracy of the same location.
[0006] A fast laser SLAM algorithm based on the intensity information matching of a quadruped robot, including:
[0007] After the multi-line laser radar scans the area, it generates point cloud data and uses the point cloud data as input to build a three-dimensional map and achieve real-time positioning;
[0008] Using the intensity information-based laser SLAM algorithm, the following steps are included:
[0009] S1. Filter out noise from point cloud data and distinguish ground points from non-ground points;
[0010] The step S1 includes the following steps:
[0011] S11. Use BFS clustering method to filter out point cloud data noise;
[0012] S12. segmenting the point cloud;
[0013] S13. Performing dimensionality reduction processing on the point cloud data;
[0014] S14. Fitting a straight line to the point cloud data after dimensionality reduction
[0015] S15. Screening ground points and non-ground points;
[0016] S2. Intensity calibration and point cloud data to extract feature points;
[0017] The step S2 includes the following steps:
[0018] S21. Calculate the intensity information of the point cloud;
[0019] S22. Extracting plane points and edge points;
[0020] S3. Motion estimation and distortion compensation;
[0021] The step S3 includes the following steps:
[0022] S31. Solve the attitude transformation matrix, each rotation of the laser radar is a frame, and construct the constraint relationship between frames;
[0023] S32. Correcting point cloud;
[0024] S4. Pose estimation;
[0025] The step S4 includes the following steps:
[0026] S41. Construct point-to-edge and point-to-face relationships;
[0027] S42. Calculate the minimum weight and pose estimation from point to edge and point to surface;
[0028] S5. Second distortion compensation and establishment of global map;
[0029] The step S5 includes the following steps:
[0030] S51. Recalculate the optimized posture transformation matrix;
[0031] S52. Update the undistorted point cloud into the map formed by the point cloud of each frame.
[0032] As a further improvement of the present invention, the step S1 is specifically as follows:
[0033] Firstly, cluster analysis is performed on the point cloud data acquired by the lidar. The BFS clustering method is used to eliminate clusters with less than 30 points, thereby filtering out the noise point cloud.
[0034] After that, the point cloud data after noise filtering is projected onto the plane xy coordinate system, and the xy plane is regarded as a circular surface divided into M sectors by Δα, so the plane is divided into M blocks:
[0035]
[0036] In order to identify where the point projected onto the xy plane is located, the following formula is used for processing:
[0037]
[0038] Among them, p i is a point with coordinates x i ,y i , z i , determine the segmentation block of each point in the xy plane;
[0039] After that, the projected coordinates are subjected to dimensionality reduction processing. Since the vertical information is lost after projection, the three-dimensional coordinate points are converted into two-dimensional coordinate points, that is, (x, y, z) = (d, z), where The coordinates after dimensionality reduction retain vertical information without losing plane information;
[0040] After obtaining the reduced-dimensional point cloud data, construct discrete points in the dz plane. For these discrete points, use y=mx+b for linear fitting in each segmentation block. The root mean square error of the fitted line should be less than the threshold TRMSE. Two thresholds are set to distinguish ground points:
[0041]
[0042] When fitting the ground, the slope must be within the threshold. If it is not within the threshold, a vertical structure will appear. At the same time, when the slope m approaches 0, the absolute value of the intercept b should not exceed the threshold Tb. A very small slope indicates that the ground is flat. The intercept represents the height of the point from the ground. By setting the threshold Tb, it can be ensured that the point cloud above the ground is not considered to be the ground. Finally, the ground points and non-ground points are distinguished based on the fitting data.
[0043] As a further improvement of the present invention, the step S2 is specifically as follows:
[0044] The received intensity information is calibrated, and the calibration result is used for feature point extraction;
[0045] 1) Strength calibration;
[0046] The intensity value is the ratio between the energy received by the reflected signal and the emitted laser power, where the received power P r for:
[0047]
[0048] P e is the transmit power, D r is the aperture of the receiver, ηsys is the system transfer coefficient, η atm is the atmospheric transfer coefficient, α is the incident angle between the object surface and the laser beam, ρ is the material reflectivity of the object, R is the measurement distance, the incident angle α is a non-intrinsic parameter, η sys and η atm is a constant parameter;
[0049] The intensity measure I is then:
[0050]
[0051] η all is a constant, so:
[0052]
[0053] Laser scanning measures distance, therefore, the local normal is analyzed to estimate the angle of incidence, and the two closest points p1 and p2 are found to calculate the normal, as shown in the following formula:
[0054]
[0055] The angle of incidence is:
[0056]
[0057] 2) Feature extraction;
[0058] When extracting feature points, the geometric information is added together with the intensity information. In the point cloud P, for each point p i , the corresponding intensity value is η i , find the point p i Nearby points N i , calculate the local distance distribution σ i G and intensity distribution
[0059]
[0060]
[0061] Where |N| is the total number of nearby points, so the features are calculated based on the following weights:
[0062]
[0063] Among them, w G is the weight of the geometric distribution, w I is the weight of the intensity distribution. When the selected point is a plane point such as a wall, the smaller the σ value is, and when the selected point is an edge point such as a corner, the larger the σ value is.
[0064] As a further improvement of the present invention, the step S3 is specifically as follows:
[0065] The feature point transformation from frame k-1 to frame k is obtained by rotation and translation transformation. After obtaining the feature points of two adjacent frames, the transformation matrix from frame k-1 to frame k is obtained. The transformation matrix at each moment can be calculated by linear interpolation during the time period required for the laser radar to rotate one circle.
[0066] Assuming that the angular velocity and linear velocity of the laser radar are constant in a very short time, when the laser radar is in frame k, the transformation matrix of the robot is T k , so the six-dimensional transformation between frames k-1 and k is:
[0067]
[0068] The interval between two frames of ξ∈se(3) is δ t The transformation matrix can be calculated by linear interpolation:
[0069]
[0070] Among them, exp(ξ) is the transformation from Lie algebra to Lie group;
[0071] The kth frame point cloud P of the current scan k The distortion is corrected by:
[0072]
[0073] As a further improvement of the present invention, the step S4 is specifically as follows:
[0074] After distortion compensation is performed on each frame;
[0075] Motion estimation is done by aligning undistorted edge features and plane features The final map is formed. The final map consists of an edge feature map and a plane feature map, which are updated and saved separately. In order to reduce the search calculation loss, the edge map and the plane map are saved in a 3D KD-tree. The lines and surfaces in the map are estimated by the nearby points in the edge feature map and the plane feature map. For each edge feature point, the covariance matrix of its nearby points is calculated. When the point is on the line, the covariance matrix contains a very large eigenvalue. The eigenvector containing a very large eigenvalue can be regarded as the direction of the line, and the direction of the line can be regarded as the geometric center of the nearby points.
[0076] Similarly, the normal vectors of the faces and surfaces in the map are obtained through the pose, and the eigenvector of the face normal vector has a minimum eigenvalue. After obtaining the edges and faces that are consistent after optimization, the edges and faces of the final map are found from each feature point in the projected point cloud. The consistency of the optimized pose is estimated by minimizing the distance between the feature points and the edges and faces in the map. The distance between the edge point and the edge in the map is:
[0077]
[0078] Among them, p ε represents the edge point, is the edge feature vector, p n for:
[0079]
[0080] The distance between the face feature and the face in the map is:
[0081]
[0082] Among them, p S represents a plane point, is the normal vector of the face;
[0083] In continuous scanning, since the extracted feature points are edge points with higher σ and plane points with lower σ, weights are introduced to balance the matching process and define local smoothness to determine the weight function. For edge points p ε The local smoothness is σ ε Plane point p S The local smoothness is σ S , the weight is:
[0084]
[0085]
[0086] in, is the set of edge features and plane features of the kth scan;
[0087] The new pose is estimated by the minimum weight sum of point-to-edge and point-to-face:
[0088] min∑W(p ε )f ε (p ε )+∑W(p S )f S (p S )
[0089] The optimized pose can be obtained by solving the nonlinear equation using the Gauss-Newton method, and the Jacobian matrix is solved using the left perturbation model:
[0090]
[0091] Among them, δξ∈se(3) is the disturbance, [T k p k ] × It is the coordinates of the point transformed from the three-dimensional coordinates {x, y, z} to the four-dimensional space, that is, {x, y, z, 1}. The Jacobian matrix of the edge residual and the Jacobian matrix of the plane residual are respectively:
[0092]
[0093]
[0094] By solving the nonlinear optimization problem, we can obtain the mileage estimation based on the above correspondence. The current pose matrix is T k * The solution can be obtained by iterative pose optimization until convergence.
[0095] As a further improvement of the present invention, the step S5 is specifically as follows:
[0096] The final map consists of an edge map and a plane map, and is updated based on key frames. When the translation change is better than the predefined translation threshold or the rotation change is better than the predefined rotation threshold, this frame is used as a key frame. Map updates based on key frames will reduce computational loss. Distortion compensation is based on a robot moving at a constant speed. However, this method is less accurate than iterative calculations. Therefore, for the optimized pose matrix T k * Recalculate and update to the final map:
[0097] Δξ * =log(T k-1 -1 T k * )
[0098]
[0099] Finally, the recalculated undistorted edge features and plane features are updated to the edges and planes in the global map respectively to form a global map.
[0100] Beneficial effects:
[0101] The invention discloses a fast laser SLAM algorithm based on intensity information matching of a quadruped robot. Since the current laser SLAM algorithm, represented by LOAM, only considers geometric information in the feature point extraction process, and the laser intensity information is often ignored in the process of laser emission and return; at the same time, the LOAM algorithm requires a large number of iterative operations in the frame-map matching process to complete the frame-map matching operation, so rich hardware resources are needed on the quadruped robot platform to complete the iterative operation, but due to the limited hardware resources on the robot platform, the algorithm has a large amount of calculation, takes a long time, and the real-time mapping effect is not good. The laser SLAM algorithm involved in the present invention has made improvements in feature point extraction and optimization algorithm complexity. In the feature extraction method involved, in addition to using geometric information, it is proposed to use the intensity of the laser as important information in feature matching. The intensity information contains the reflectivity of the environmental structure, and the environmental structure shows the distribution of different objects. Therefore, the intensity information helps to identify the same features in multiple frames; at the same time, the algorithm motion estimation module is improved. After obtaining the pose estimation, the distortion is calculated again, and the features that are not distorted after calculation are updated to the map, so as to improve the positioning accuracy of the same location. BRIEF DESCRIPTION OF THE DRAWINGS
[0102] Figure 1 It is a specific flow chart of the present invention. DETAILED DESCRIPTION
[0103] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments:
[0104] The present invention provides a fast laser SLAM algorithm based on intensity information. The intensity information is based on the power change information emitted and returned by the laser radar. Considering that most algorithms only consider the geometric information of the point cloud and ignore the intensity information of the laser in the feature matching process, it is proposed to add the intensity information; the frame-map matching and motion estimation module, due to the limited resources of the quadruped robot platform, and the large amount of calculation in the frame-map matching process, the long running time, and the poor real-time performance, consider improving the matching algorithm in the frame-map matching process, after obtaining the pose estimation, recalculate the distortion, update the calculated undistorted features to the map, and improve the positioning accuracy of the same location. The specific process is as follows Figure 1 Show.
[0105] In the present invention, the laser radar obtains the laser point cloud of the surrounding environment, converts the point cloud information into Euclidean distance, and each point has a unique coordinate point. Then the point cloud data is segmented to distinguish between ground points and non-ground points, and then features are extracted, including plane points and edge points, and the intensity information of the calculated point cloud is calculated. Then feature matching is performed, and after matching, the posture is obtained, the distortion is calculated again, and the undistorted features are updated to the map.
[0106] (a) Point cloud processing.
[0107] Firstly, cluster analysis is performed on the point cloud data acquired by the lidar, and the BFS clustering method is used to eliminate clusters with less than 30 points, thereby filtering out the noise point cloud.
[0108] After that, the point cloud data after noise filtering is projected onto the plane xy coordinate system, and the xy plane is regarded as a circular surface divided into M sectors by Δα. Therefore, the plane is divided into M blocks:
[0109]
[0110] In order to identify where the point projected onto the xy plane is located, the following formula is used for processing:
[0111]
[0112] Among them, p i is a point with coordinates (x i ,y i , z i ), so that the segmentation block of each point in the xy plane can be determined.
[0113] After that, the projected coordinates are subjected to dimensionality reduction processing. Since the vertical information is lost after projection, the three-dimensional coordinate points are converted into two-dimensional coordinate points, that is, (x, y, z) = (d, z), where The coordinates after dimensionality reduction retain the vertical information without losing the plane information.
[0114] After obtaining the reduced-dimensional point cloud data, construct discrete points on the dz plane, and use y=mx+b to perform linear fitting on these discrete points in each segmentation block. The root mean square error of the fitted line should be less than the threshold T RMSE , by setting two thresholds to distinguish ground points:
[0115]
[0116] The slope of the ground fitting should be within the threshold. If it is not within the threshold, a vertical structure will appear. At the same time, when the slope m approaches 0, the absolute value of the intercept b should not exceed the threshold T. b , when the slope is very small, it means the ground is flat, and the intercept represents the height of the point from the ground. Set the threshold T b , it can ensure that the point cloud above the ground will not be considered as the ground. Finally, the ground points and non-ground points are distinguished based on the fitting data.
[0117] (II) Intensity calibration and feature point extraction
[0118] The received intensity information is calibrated, and the calibration result is used for feature point extraction.
[0119] 1. Strength calibration.
[0120] The intensity value is the ratio between the energy received by the reflected signal and the emitted laser power. r for:
[0121]
[0122] P e is the transmit power, D r is the aperture of the receiver, η sys is the system transfer coefficient, η atm is the atmospheric transfer coefficient, α is the incident angle between the object surface and the laser beam, ρ is the material reflectivity of the object, R is the measurement distance, the incident angle α is a non-intrinsic parameter, η sys and η atm is a constant parameter.
[0123] The intensity measure I is then:
[0124]
[0125] η all is a constant, so:
[0126]
[0127] Laser scanning can easily measure distance, so the local normal can be analyzed to estimate the angle of incidence. The normal can be calculated by finding the two closest points p1 and p2, as shown in the following formula:
[0128]
[0129] The angle of incidence is:
[0130]
[0131] 2. Feature extraction.
[0132] When extracting feature points, the geometric information is added together with the intensity information. In the point cloud P, for each point p i , the corresponding intensity value is η i , find the point p i Nearby points N i , calculate the local distance distribution σ i G and intensity distribution
[0133]
[0134]
[0135] Among them, |N| is the total number of nearby points, so the features can be calculated according to the weights of the following formula:
[0136] σ i =w G ·σ i G +w I ·σ i I
[0137] Among them, w G is the weight of the geometric distribution, w I is the weight of the intensity distribution. When the selected point is a plane point such as a wall, the smaller the σ value is, and when the selected point is an edge point such as a corner, the larger the σ value is.
[0138] 3. Motion Estimation and Distortion Compensation
[0139] The transformation of feature points from frame k-1 to frame k is obtained by rotation and translation. After obtaining the feature points of two adjacent frames, the transformation matrix from frame k-1 to frame k can be obtained. The transformation matrix at each moment can be calculated by linear interpolation during the time period required for the laser radar to rotate one circle.
[0140] Since most laser radars can run at more than 10 Hz and the scanning time interval between two consecutive frames is very short, it can be assumed that the angular velocity and linear velocity of the laser radar are constant in a very short time. When the laser radar is in frame k, the robot's transformation matrix is T k , so the six-dimensional transformation between frames k-1 and k is:
[0141]
[0142] The interval between two frames of ξ∈se(3) is δ t The transformation matrix can be calculated by linear interpolation:
[0143]
[0144] Here, exp(ξ) is the transformation from Lie algebra to Lie group.
[0145] The kth frame point cloud P of the current scan k The distortion can be corrected by:
[0146]
[0147] (IV) Pose Estimation
[0148] After distortion compensation for each frame
[0149] Motion estimation is done by aligning undistorted edge features and plane features The final map is formed. The final map consists of an edge feature map and a plane feature map, which are updated and saved separately. In order to reduce the search calculation loss, the edge map and the plane map are saved in a 3D KD-tree. The lines and surfaces in the map are estimated by the nearby points in the edge feature map and the plane feature map. For each edge feature point, the covariance matrix of its nearby points is calculated. When the point is on the line, the covariance matrix contains a very large eigenvalue. The eigenvector containing a very large eigenvalue can be regarded as the direction of the line, and the direction of the line can be regarded as the geometric center of the nearby points.
[0150] Similarly, the normal vectors of the faces and surfaces in the map can be obtained through the pose, and the eigenvector of the face normal vector has a minimum eigenvalue. After obtaining the edges and faces that are consistent after optimization, the edges and faces of the final map can be found from each feature point in the projected point cloud. The consistency of the optimized pose is estimated by minimizing the distance between the feature points and the edges and faces in the map. The distance between the edge point and the edge in the map is:
[0151]
[0152] Among them, p ε represents the edge point, is the edge feature vector, p n for:
[0153]
[0154] The distance between the polygon feature and the polygon in the map is:
[0155]
[0156] Among them, p S represents a plane point, is the normal vector of the face.
[0157] In continuous scanning, since the extracted feature points are edge points with higher σ and plane points with lower σ, weights are introduced to balance the matching process. Local smoothness is defined to determine the weight function. For edge points p ε The local smoothness is σ ε Plane point p S The local smoothness is σ S , the weight is:
[0158]
[0159]
[0160] in, is the set of edge features and plane features of the kth scan.
[0161] The new pose can be estimated by the minimum weighted sum of point-to-edge and point-to-face:
[0162] minΣW(p ε )f ε (p ε )+ΣW(p S )f S (p S )
[0163] The optimized pose can be obtained by solving the nonlinear equations using the Gauss-Newton method. The Jacobian matrix is solved using the left perturbation model:
[0164]
[0165] Among them, δξ∈se(3) is the disturbance, [T k p k ] × It is the coordinates of the point transformed from the three-dimensional coordinates {x, y, z} to the four-dimensional space, that is, {x, y, z, 1}. The Jacobian matrix of the edge residual and the Jacobian matrix of the plane residual are respectively:
[0166]
[0167]
[0168] By solving the nonlinear optimization problem, we can get the mileage estimation based on the above correspondence. The current pose matrix is T k * The solution can be obtained by iterative pose optimization until convergence.
[0169] (V) Secondary distortion compensation and establishment of global map
[0170] The final map consists of an edge map and a plane map, and is updated based on key frames. When the translation change is better than the predefined translation threshold or the rotation change is better than the predefined rotation threshold, this frame is used as a key frame. Map updates based on key frames will reduce computational loss. Distortion compensation is based on a robot moving at a constant speed. However, this method is less accurate than iterative calculations. Therefore, for the optimized pose matrix T k * Recalculate and update to the final map:
[0171] Δξ * =log(T k-1 -1 T k * )
[0172]
[0173] Finally, the recalculated undistorted edge features and plane features are updated to the edges and planes in the global map respectively to form a global map.
[0174] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention still falls within the scope of protection required by the present invention.
Claims
1. A fast laser SLAM algorithm based on intensity information matching of a quadruped robot, comprising, characterized in that: After the multi-line laser radar scans the area, it generates point cloud data and uses the point cloud data as input to build a three-dimensional map and achieve real-time positioning; Using the intensity information-based laser SLAM algorithm, the following steps are included: S1. Filter out noise from point cloud data and distinguish ground points from non-ground points; The step S1 includes the following steps: S11. Use BFS clustering method to filter out point cloud data noise; S12. segmenting the point cloud; S13. Performing dimensionality reduction processing on the point cloud data; S14. Fitting a straight line to the point cloud data after dimensionality reduction S15. Screening ground points and non-ground points; S2. Intensity calibration and point cloud data to extract feature points; The step S2 includes the following steps: S21. Calculate the intensity information of the point cloud; S22. Extracting plane points and edge points; S3. Motion estimation and distortion compensation; The step S3 includes the following steps: S31. Solve the attitude transformation matrix, each rotation of the laser radar is a frame, and construct the constraint relationship between frames; S32. Correcting point cloud; S4. Pose estimation; The step S4 includes the following steps: S41. Construct point-to-edge and point-to-face relationships; S42. Calculate the minimum weight and pose estimation from point to edge and point to surface; S5. Second distortion compensation and establishment of global map; The step S5 includes the following steps: S51. Recalculate the optimized posture transformation matrix; S52. Update the undistorted point cloud into the map formed by the point cloud of each frame.
2. The fast laser SLAM algorithm based on the intensity information matching of the quadruped robot according to claim 1, characterized in that: The step S1 is specifically as follows: Firstly, cluster analysis is performed on the point cloud data acquired by the lidar. The BFS clustering method is used to eliminate clusters with less than 30 points, thereby filtering out the noise point cloud. After that, the point cloud data after noise filtering is projected onto the plane xy coordinate system, and the xy plane is regarded as a circular surface divided into M sectors by Δα, so the plane is divided into M blocks: In order to identify where the point projected onto the xy plane is located, the following formula is used for processing: Among them, p i is a point with coordinates x i ,y i , z i , determine the segmentation block of each point in the xy plane; After that, the projected coordinates are subjected to dimensionality reduction processing. Since the vertical information is lost after projection, the three-dimensional coordinate points are converted into two-dimensional coordinate points, that is, (x, y, z) = (d, z), where The coordinates after dimensionality reduction retain vertical information without losing plane information; After obtaining the reduced-dimensional point cloud data, construct discrete points in the dz plane. For these discrete points, use y=mx+b for linear fitting in each segmentation block. The root mean square error of the fitted line should be less than the threshold TRMSE. Two thresholds are set to distinguish ground points: When fitting the ground, the slope must be within the threshold. If it is not within the threshold, a vertical structure will appear. At the same time, when the slope m approaches 0, the absolute value of the intercept b should not exceed the threshold Tb. A very small slope indicates that the ground is flat. The intercept represents the height of the point from the ground. By setting the threshold Tb, it can be ensured that the point cloud above the ground is not considered to be the ground. Finally, the ground points and non-ground points are distinguished based on the fitting data.
3. The fast laser SLAM algorithm based on the intensity information matching of the quadruped robot according to claim 1, characterized in that: The step S2 is specifically as follows: The received intensity information is calibrated, and the calibration result is used for feature point extraction; 1) Strength calibration; The intensity value is the ratio between the energy received by the reflected signal and the emitted laser power, where the received power P r for: P e is the transmit power, D r is the aperture of the receiver, η sys is the system transfer coefficient, η atm is the atmospheric transfer coefficient, α is the incident angle between the object surface and the laser beam, ρ is the material reflectivity of the object, R is the measurement distance, the incident angle α is a non-intrinsic parameter, η sys and η atm is a constant parameter; The intensity measure I is then: η all is a constant, so: Laser scanning measures distance, therefore, the local normal is analyzed to estimate the angle of incidence, and the two closest points p1 and p2 are found to calculate the normal, as shown in the following formula: The angle of incidence is: 2) Feature extraction; When extracting feature points, the geometric information is added together with the intensity information. In the point cloud P, for each point p i , the corresponding intensity value is η i , find the point p i Nearby points N i , calculate the local distance distribution and intensity distribution Where |N| is the total number of nearby points, so the features are calculated based on the following weights: Among them, w G is the weight of the geometric distribution, w I is the weight of the intensity distribution. When the selected point is a plane point such as a wall, the smaller the σ value is, and when the selected point is an edge point such as a corner, the larger the σ value is.
4. The fast laser SLAM algorithm based on the intensity information matching of the quadruped robot according to claim 1, characterized in that: The step S3 is specifically as follows: The transformation of feature points from frame k-1 to frame k is obtained by rotation and translation. After obtaining the feature points of two adjacent frames, the transformation matrix from frame k-1 to frame k is obtained. The transformation matrix at each moment can be calculated by linear interpolation during the time period required for the laser radar to rotate one circle. Assuming that the angular velocity and linear velocity of the laser radar are constant in a very short time, when the laser radar is in frame k, the transformation matrix of the robot is T k , so the six-dimensional transformation between frames k-1 and k is: The interval between two frames of ξ∈se(3) is δ t The transformation matrix can be calculated by linear interpolation: Among them, exp(ξ) is the transformation from Lie algebra to Lie group; The kth frame point cloud P of the current scan k The distortion is corrected by:
5. The fast laser SLAM algorithm based on the intensity information matching of the quadruped robot according to claim 1, characterized in that: The step S4 is specifically as follows: After distortion compensation is performed on each frame; Motion estimation is done by aligning undistorted edge features and plane features The final map is composed of edge feature maps and plane feature maps. Update and save them separately; in order to reduce the search calculation loss, the edge map and plane map are saved in the 3D KD-tree. The lines and surfaces in the map are estimated by the nearby points in the edge feature map and the plane feature map. For each edge feature point, the covariance matrix of its nearby points is calculated. When the point is on the line, the covariance matrix contains a very large eigenvalue. The eigenvector containing a very large eigenvalue can be regarded as the direction of the line, and the direction of the line can be regarded as the geometric center of the nearby points; Similarly, the normal vectors of the faces and surfaces in the map are obtained through the pose, and the eigenvector of the face normal vector has a minimum eigenvalue. After obtaining the edges and faces that are consistent after optimization, the edges and faces of the final map are found from each feature point in the projected point cloud. The consistency of the optimized pose is estimated by minimizing the distance between the feature points and the edges and faces in the map. The distance between the edge point and the edge in the map is: Among them, p ε represents the edge point, is the edge feature vector, p n for: The distance between the polygon feature and the polygon in the map is: Among them, p S represents a plane point, is the normal vector of the face; In continuous scanning, since the extracted feature points are edge points with higher σ and plane points with lower σ, weights are introduced to balance the matching process and define local smoothness to determine the weight function. For edge points p ε The local smoothness is σ ε Plane point p S The local smoothness is σ S , the weight is: in, is the set of edge features and plane features of the kth scan; The new pose is estimated by the minimum weight sum of point-to-edge and point-to-face: minΣW(p ε )f ε (p ε )+ΣW(p S )f S (p S ) The optimized pose can be obtained by solving the nonlinear equation using the Gauss-Newton method, and the Jacobian matrix is solved using the left perturbation model: Among them, δξ∈se(3) is the disturbance, [T k p k ] × It is the coordinates of the point transformed from the three-dimensional coordinates {x, y, z} to the four-dimensional space, that is, {x, y, z, 1}. The Jacobian matrix of the edge residual and the Jacobian matrix of the plane residual are respectively: By solving the nonlinear optimization problem, we can obtain the mileage estimation based on the above correspondence. The current pose matrix is: The solution can be obtained by iterative pose optimization until convergence.
6. The fast laser SLAM algorithm based on the intensity information matching of the quadruped robot according to claim 1, characterized in that: The step S5 is specifically as follows: The final map consists of an edge map and a plane map, and is updated based on key frames. When the translation change is better than the predefined translation threshold or the rotation change is better than the predefined rotation threshold, this frame is used as a key frame. Map updates based on key frames will reduce computational loss. Distortion compensation is based on a robot moving at a constant speed. However, this method is less accurate than iterative calculations. Therefore, for the optimized pose matrix Recalculate and update to the final map: Finally, the recalculated undistorted edge features and plane features are updated to the edges and planes in the global map respectively to form a global map.
