Slam positioning system and method based on reflective stripe enhancement

Abstract

The invention relates to a slam positioning system and method based on reflective stripe enhancement. The method comprises the steps of system mapping; feature extraction and classification; combining the relative pose change of adjacent frames with speedometer data output poses, screening key frames according to a threshold value, and adding constraint factors; key frames near the current pose are detected and registered, loops are screened according to the threshold matching degree, and closed-loop factors are added for optimization to obtain historical key frame poses; storing map data; the method further comprises the steps of system positioning; map loading; extracting features; receiving speedometer and feature point cloud data, predicting and calculating a current pose by the speedometer, and converting the current pose to a map coordinate system; current frame point cloud feature points are matched and errors are calculated; and calculating and releasing a transformation pose. According to the method, a layered processing mechanism is adopted, a parallel matching strategy of the intensity threshold reflective stripe features and geometric feature angular points and side points is adopted, when a dynamic object shields the reflective stripe, a geometric feature matching mode is automatically switched, continuous positioning is kept through curvature self-adaption angular point and side point features, and accumulated errors are effectively restrained.

Description

Technical Field

[0001] The present invention relates to the technical field of robot autonomous navigation and positioning, and in particular to a SLAM positioning system and method based on enhanced reflective strips. Background Art

[0002] With the rapid development of mobile robot technology, two-dimensional laser SLAM technology faces significant challenges in environmental modeling and autonomous positioning. Traditional algorithms based on point cloud matching have inherent defects in dynamic scenarios: First, dynamic object interference causes significant deviations between real-time laser point clouds and the original map, and cumulative errors are difficult to eliminate; Second, in single-feature environments such as long corridors and empty warehouses, due to the lack of geometric diversity features, effective loop closures cannot be established, resulting in divergent pose estimation. More seriously, existing positioning systems based on reflector assistance require at least three non-collinear reflectors to solve the pose, which not only increases the deployment cost but also is more likely to cause positioning failure in scenarios with frequent dynamic occlusions. Summary of the Invention

[0003] Aiming at the above-mentioned shortcomings of the prior art, the purpose of the present invention is to provide a SLAM positioning system and method based on enhanced reflective strips to solve one or more problems in the prior art.

[0004] To achieve the above purpose, the technical solution of the present invention is as follows: A SLAM positioning system based on enhanced reflective strips, the system includes an AGV main body, a processor, an odometer, and a radar are arranged in the AGV main body, the processor acts on the odometer and the radar respectively, the system further includes reflective strips, and the radar acts on the reflective strips.

[0005] A positioning method, the positioning method is applied to the above positioning system, and the method includes the following steps: System mapping, including the following steps: Feature extraction and classification, including three types of features: highly reflective points, corner points, and edge points screened based on a set threshold, Based on the relative pose change between adjacent frames combined with odometer data, the pose is output in real time, and at the same time, key frames are screened based on a set threshold and constraint factors are added, Detect and register key frames near the current pose and screen for loop closures based on a set threshold matching degree, add loop closure factors and optimize to obtain the pose of historical key frames, Map data storage, including key frame poses, highly reflective points, corner points, and edge points; System positioning, including the following steps: Map loading, Feature extraction, including highly reflective points, corner points, and edge points, Receive odometry and feature point cloud data, predict and calculate the current pose using odometry, and transform the features into the map coordinate system. Match the feature points in the current frame of point cloud and calculate the matching error and total error. Calculate the transformed pose and publish it.

[0006] Furthermore, the highly reflective points are the points where the return intensity of the radar to the material is greater than the set threshold, the corner points are the points where the calculated curvature of adjacent radar points is greater than the set threshold, and the edge points are the points where the calculated curvature of adjacent radar points is lower than the set threshold. The calculation formula of the curvature is as follows:

[0007] In the formula: S is the set of consecutive points returned by the laser in the same frame. refers to the i-th point in the point cloud Pk of the k-th scan in the radar L coordinate system. Among them, if the c value is large, it means that the difference between the current point and the surrounding points is large, and the curvature is high, representing a corner point; if the c value is small, it means that the difference between the current point and the surrounding points is small, and the curvature is low, representing an edge point.

[0008] Furthermore, in the screening of key frames based on the set threshold and adding constraint factors, it is judged based on the moving distance and rotation angle of the AGV body. If it exceeds the set threshold, the features and pose in the current frame are saved as key frames; if not, no processing is done.

[0009] Furthermore, the loop closure screening based on the matching degree compared with the set threshold is implemented based on the ICP algorithm. If the matching degree is higher than the set threshold, loop closure is performed; if it is lower than the set threshold, no processing is done.

[0010] Furthermore, the calculation formula for transforming the features into the map coordinate system is as follows:

[0011] In the formula: represents the current transformation matrix. represents the previous transformation matrix. represents the relative transformation matrix provided by the odometry from the previous moment to this moment.

[0012] Furthermore, the matching of the feature points in the current frame of point cloud is based on the feature points pi in the current frame of point cloud, and the nearest neighbor point qi is searched in the global point cloud. The calculation formula is as follows:

[0013] In the formula: Points in the map point cloud. Points in the current frame of point cloud, argmin refers to the minimum input value of the function. are the feature points in the global point cloud.

[0014] Furthermore, in the calculation of the matching error, the calculation formula of the matching error of the highly reflective points is as follows:

[0015] In the formula: R is the rotation matrix, and t is the translation matrix; The calculation formula of the corner point matching error is as follows:

[0016] The calculation formula of the edge point matching error is as follows:

[0017] In the formula: is the unit vector corresponding to the straight line formed by the neighboring corner points.

[0018] Furthermore, the total error is the weighted sum of the matching errors of each feature point, and the calculation formula is as follows:

[0019] In the formula: , and are the weights corresponding to the errors of the reflective points, corner points, and variable points respectively.

[0020] Compared with the prior art, the beneficial technical effects of the present invention are as follows: (1) Through the enhanced hybrid feature hierarchical processing mechanism of the reflective strip in the dynamic scene, a parallel matching strategy of the reflective strip feature based on the intensity threshold and the geometric features of corner points and edge points is adopted. When a dynamic object blocks the reflective strip, the system automatically switches to the geometric feature matching mode, and the positioning continuity is maintained through the corner point and edge point features with curvature adaptability, effectively suppressing the cumulative error.

[0021] (2) Through the tight coupling of the reflective strip and the odometer, the pose solution can still be achieved through the triple constraints of the reflective feature, geometric feature, and motion model when there is only a single reflective strip, significantly reducing the deployment cost; also, a joint constraint of the reflective strip feature and the natural feature is established through the factor graph optimization framework gtsam to solve the problem of loop closure failure caused by single geometric features in scenarios such as long corridors. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 shows a schematic flow diagram of a slam positioning system and method based on reflective strip enhancement according to an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0024] In order to make the objectives, technical solutions and advantages of the present invention clearer and more understandable, the following further elaborates in detail on a SLAM positioning system and method based on reflective strip enhancement proposed by the present invention in combination with the accompanying drawings and specific embodiments. According to the following description, the advantages and features of the present invention will be clearer. It should be noted that the accompanying drawings are in a very simplified form and all use non-precise scales, only for conveniently and clearly assisting in explaining the objectives of the embodiments of the present invention. In order to make the objectives, features and advantages of the present invention more obvious and understandable, please refer to the accompanying drawings. It should be noted that the structures, scales, sizes, etc. shown in the drawings of this specification are only used to cooperate with the content disclosed in the specification for those familiar with this technology to understand and read, and are not used to limit the limiting conditions for the implementation of the present invention. Therefore, they do not have substantial technical significance. Any modification of the structure, change of the proportional relationship or adjustment of the size, without affecting the effects that the present invention can produce and the objectives that can be achieved, should still fall within the scope covered by the technical content disclosed by the present invention.

[0025] Please refer to Figure 1 , a SLAM positioning system based on reflective strip enhancement, the system includes an AGV main body, a processor, an odometer and a radar are arranged in the AGV main body, and the processor acts on the odometer and the radar respectively. The system also includes a reflective strip, and the radar acts on the reflective strip.

[0026] A positioning method, the positioning method is applied to the above positioning system, and the method includes the following steps: Step 1: System mapping, including the following steps: Step 1.1: Feature extraction and classification, including three types of features: high-reflection points, corner points and edge points screened based on a set threshold. The high-reflection points are the points where the return intensity of the radar to the material is greater than the set threshold. The corner points are the points where the calculated curvature of adjacent radar points is greater than the set threshold. The edge points are the points where the calculated curvature of adjacent radar points is lower than the set threshold. The setting and selection of the above thresholds will be user-defined according to the model of the radar used and the reflective material. The calculation formula of the curvature is shown in Formula 1 below:

[0027] In the formula: S is the set of consecutive points returned by the laser in the same frame, refers to the i-th point in the point cloud Pk of the k-th scan in the radar L coordinate system.

[0028] If the value of c is large, it means that the current point has a large difference from the surrounding points, and the curvature is high, representing a corner point. If the value of c is small, it means that the current point has a small difference from the surrounding points, and the curvature is low, representing an edge point. The judgment of the size of the C value is based on the user-defined threshold.

[0029] Step 1.2: Based on the relative pose change between adjacent frames combined with odometer data, the pose is output in real time. The relative pose change between adjacent frames is obtained by calculating the point cloud matching between frames, that is, it is realized through the well-known ICP point cloud matching algorithm. Since frame matching takes time, in order to ensure the continuity of the pose, the odometer data is combined, and then a continuous global pose trajectory is generated. At the same time, key frames are screened based on a set threshold and constraint factors are added. The screening of key frames is judged based on the moving distance and rotation angle of the AGV body. If it exceeds the set threshold, the high-reflectivity points, corner points, edge points and pose in the current frame are saved as 1 key frame, and frame-to-frame constraint factors are added to the well-known factor graph optimization library gtsam for optimization processing. If it does not exceed, no processing is done.

[0030] Step 1.3: Regularly detect and register the key frames near the current pose, and screen for loop closure based on the matching degree compared with the set threshold. Add loop closure factors and optimize to obtain the poses of historical key frames. The screening for loop closure based on the matching degree compared with the set threshold is implemented based on the ICP algorithm. If the matching degree is higher than the set threshold, loop closure is performed, and loop closure factors are added to the factor graph optimization library gtsam. Then gtsam combines the constraint factors and loop closure factors to perform optimization. If it is lower than the set threshold, no processing is done.

[0031] Step 1.4: Save the map data, including the poses of key frames, high-reflectivity points, corner points, and edge points.

[0032] Step 2: System positioning, including the following steps: Step 2.1: Map loading.

[0033] Step 2.2: Feature extraction. Through radar feature extraction, high-reflectivity points are extracted according to intensity, and corner points and edge points are extracted according to curvature.

[0034] Step 2.3: Receive odometer and feature point cloud data. Use the odometer as the predicted value to calculate the current pose, transform the features to the map coordinate system, and the calculation formula is as shown in Equation 2 below:

[0035] In the formula: represents the current transformation matrix, represents the transformation matrix at the previous moment, represents the relative transformation matrix provided by the odometer from the previous moment to this moment.

[0036] The current pose predicted by the above Equation 2 is the transformation matrix. Then multiply the feature point cloud by the obtained transformation matrix to complete the transformation.

[0037] Step 2.4: Match the feature points in the current frame point cloud and calculate the matching error and total error.

[0038] The matching of feature points in the current frame point cloud is based on the feature point pi in the current frame point cloud. The nearest neighbor point qi is searched in the global point cloud through KD-Tree, and the calculation formula is shown in Formula 3 below:

[0039] In the formula: Points in the map point cloud, Points in the current frame point cloud, argmin refers to the minimum input value of the function, are the respective feature points in the global point cloud.

[0040] Among them, for corner point matching, directly find the nearest neighbor corner point. For highly reflective point matching, directly find the nearest neighbor highly reflective point. For edge point matching, find multiple nearest neighbor points and fit a straight line, and calculate the distance from the point to the straight line.

[0041] Furthermore, calculate the matching error, and add it to the nonlinear optimization library ceres by constructing a cost function.

[0042] In the calculation of the matching error, the calculation formula for the matching error of the highly reflective point is shown in Formula 4 below:

[0043] In the formula: R is the rotation matrix, and t is the translation matrix.

[0044] The calculation formula for the corner point matching error is shown in Formula 5 below:

[0045] The calculation formula for the edge point matching error is shown in Formula 6 below:

[0046] In the formula: is the unit vector corresponding to the straight line composed of neighboring corner points.

[0047] Furthermore, calculate the total error, which is the weighted sum of the matching errors of each feature point. The calculation formula is shown in Formula 7 below:

[0048] In the formula: , and are the weights corresponding to the reflective point, corner point, and edge point errors respectively.

[0049] Step 2.5: Calculate the transformation pose and publish it.

[0050] Using well-known techniques, the transformation matrix (R, t) is calculated by minimizing the error E. Based on least squares optimization, the transformation matrix obtained by the QR decomposition of the ceres library is the correct pose, and then the current positioning of the AGV is updated and published.

[0051] That is, the pose is estimated by minimizing the feature point matching error, and the goal is to minimize the error function E(R, t).

[0052] Using the iterative optimization algorithm in the ceres library, in each iteration, the residual function is linearized to obtain the Jacobian matrix J, that is, the derivative of the residual with respect to the parameters R and t.

[0053] Furthermore, the optimization problem is transformed into solving a linear equation system, as shown in Equation 8 below:

[0054] In the formula: is the parameter increment, that is, the update amount of rotation and translation.

[0055] Furthermore, the above linear system is solved by QR decomposition, specifically as follows: The Jacobian matrix J is decomposed into an orthogonal matrix Q and an upper triangular matrix R, that is, J = QR.

[0056] Since Q is orthogonal, that is, Q T Q = I, and the simplified solution transforms the original equation into the form shown in Equation 9 below:

[0057] The upper triangular matrix R is efficiently solved by back substitution to obtain the parameter increment and then update R and t.

[0058] By repeating the iteration until convergence, the optimal pose is obtained.

[0059] The technical features of the above-described embodiments can be combined arbitrarily. For the sake of brevity of description, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, it should be considered as the scope recorded in this specification.

[0060] The above-described embodiments only represent several implementation manners of the present invention, and their descriptions are relatively specific and detailed, but they should not be construed as limiting the scope of the invention patent. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present invention, several deformations and improvements can still be made, and these all belong to the protection scope of the present invention. Therefore, the protection scope of the present invention patent should be subject to the appended claims.

Claims

1. A SLAM positioning system enhanced based on reflective strips, characterized in that: The system includes an AGV body, in which a processor, an odometer, and a radar are provided, and the processor acts on the odometer and the radar respectively; the system further includes a reflective strip, and the radar acts on the reflective strip.

2. A SLAM positioning method based on enhanced reflective strips, the positioning method being applied to the positioning system as described in claim 1, characterized in that: The method includes the following steps: System mapping, including the following steps: Feature extraction and classification, including three types of features: high-reflection points, corner points, and edge points screened based on a set threshold, Based on the relative pose change between adjacent frames and combined with odometer data, the pose is output in real time, and at the same time, key frames are screened based on a set threshold and constraint factors are added, Detect and register key frames near the current pose and screen for loop closure based on the matching degree compared with the set threshold, add loop closure factors and optimize to obtain the pose of historical key frames, Map data saving, including the pose of key frames, high-reflection points, corner points, and edge points; System positioning, including the following steps: Map loading, Feature extraction, including high-reflection points, corner points, and edge points, Receive odometer and feature point cloud data, predict and calculate the current pose using the odometer, and transform the features into the map coordinate system, Match the feature points in the current frame point cloud and calculate the matching error and the total error, Calculate the transformed pose and publish it.

3. The SLAM positioning method based on reflective strip enhancement according to claim 2, wherein: The high-reflection points are the points where the return intensity of the radar on the material is greater than the set threshold, the corner points are the points where the calculated curvature of adjacent radar points is greater than the set threshold, the edge points are the points where the calculated curvature of adjacent radar points is lower than the set threshold, and the calculation formula of the curvature is shown as follows: ， Where: S is a set of consecutive points returned by the laser in the same frame, refers to the i-th point in the point cloud Pk of the k-th scan in the radar L coordinate system; Where if the c value is large, it means that the difference between the current point and the surrounding points is large, the curvature is high, and it represents a corner point; if the c value is small, it means that the difference between the current point and the surrounding points is small, the curvature is low, and it represents an edge point.

4. The SLAM positioning method based on enhanced reflective strips according to claim 3, wherein: In the screening of key frames based on a set threshold and adding constraint factors, it is judged based on the moving distance and rotation angle of the AGV body. If it exceeds the set threshold, the features and pose in the current frame are saved as key frames. If it does not exceed, no processing is done.

5. The SLAM positioning method based on the enhancement of reflective strips according to claim 4, wherein: The screening for loop closure based on the matching degree compared with the set threshold is implemented based on the ICP algorithm. If the matching degree is higher than the set threshold, loop closure is performed. If it is lower than the set threshold, no processing is done.

6. The SLAM positioning method based on the enhancement of reflective strips according to claim 5, wherein: The calculation formula for transforming the features into the map coordinate system is shown as follows: ， In the formula: represents the current transformation matrix, represents the previous transformation matrix, represents the relative transformation matrix provided by the odometer from the previous moment to this moment.

7. The SLAM positioning method based on the enhancement of reflective strips according to claim 6, characterized in that: The matching of the feature points in the current frame point cloud is based on the feature points pi in the current frame point cloud, and the nearest neighbor points qi are searched in the global point cloud. The calculation formula is shown as follows: ， Wherein: Points in the map point cloud Points in the current frame point cloud, and argmin refers to the minimum input value of the function Are the respective feature points in the global point cloud 8. The SLAM positioning method based on the enhancement of reflective strips according to claim 7, characterized in that: In the calculation of the matching error, the calculation formula for the matching error of the high-reflection points is shown as follows: ， In the formula: R is the rotation matrix, and t is the translation matrix; The calculation formula for the corner point matching error is shown as follows: ， The calculation formula for the edge point matching error is shown as follows: ， Wherein: is the unit vector corresponding to the straight line formed by adjacent corner points.

9. The SLAM positioning method based on the enhancement of reflective strips according to claim 8, wherein: The total error is the weighted sum of the matching errors of each feature point. The calculation formula is shown as follows: ， Wherein: , and are the weights corresponding to the reflection point, the corner point and the variable point error respectively.

Images