Method for improving 2D-SLAM precision and stability based on characteristic road sign
Technical Field
The invention belongs to the technical field of instant positioning and map construction, and particularly relates to a method for improving the precision and stability of a 2D-SLAM based on a characteristic road sign.
Background
At present, SLAM (instant positioning and mapping technology) is applied more and more widely in the mobile robot industry, the 2D-SLAM technology refers to plane mapping and positioning, compared with 3D-SLAM, the environment characteristics are few, the advantages are that excessive complex calculation processing is not needed, a gyroscope accelerometer sensor is not needed for gravity correction, the defect is that the positioning loss is possibly caused due to the fact that the information amount is small, at present, 2 DSLAMs are mostly applied to indoor scenes, the used sensor is a single-line laser radar with 360 degrees or 270 degrees, the open source algorithm for mapping is also various, and a sector, a mapping and a cartographer positioning and mapping algorithm are common.
Many open source algorithms cannot meet actual requirements, the robustness of positioning is not guaranteed, and the positioning accuracy cannot meet the requirements of environments such as factories and the like, so algorithm improvement is needed; and other modes such as a 2D-SLAM fusion reflector or ultrasonic waves and the like are adopted, so that the system is relatively complex.
Disclosure of Invention
The invention aims to provide a method for improving the precision and stability of the 2D-SLAM based on the characteristic road sign in order to solve the problems that the robustness of the 2D-SLAM technology for positioning is not ensured and the positioning precision is low, and the method has the advantages of robustness and precision of robot positioning and stable and reliable pose output.
The invention realizes the aim through the following technical scheme, and a method for improving the precision and the stability of a 2D-SLAM based on a characteristic road sign comprises the following steps:
in the process of constructing a map in a laser scanning environment, clustering laser points to generate a cluster point set, and taking an environment with densely distributed cluster points as a stable environment;
performing outlier calculation on laser clustering points in a stable environment, extracting a clustering point set which accords with a characteristic contour as a characteristic road sign, and storing data of the clustering point set into a characteristic road sign set;
in the positioning process, scanning out characteristic road signs according to laser poses, inquiring absolute poses in a road sign set, matching laser point clouds after conditions are met to obtain a pose transformation matrix, updating the current characteristic road signs in the characteristic road sign set, and deleting the original road signs;
and correcting the pose in the constructed environment map according to the pose variation.
Preferably, the clustering process is to cluster the laser points in the laser cloud, and the threshold between adjacent points in the same cluster is set to be 1cm, and the threshold between distances in different clusters is set to be 5 m.
Preferably, the stable environment is a wall surface and/or a corner.
Preferably, the method for extracting the feature profile includes:
a. performing two times of ransac fitting on the candidate feature point set with the same class set larger than a certain number of points;
b. first fitting, namely, if the distance value under the same set is increased and then decreased within a certain distance range, the feature contour points are obtained;
c. and performing secondary ransac fitting to calculate the number of outliers, if the number is too small, determining that the outliers do not meet the condition, verifying the number of dichotomous points again, performing linear fitting once for each part, if the difference between the slopes of the two straight lines is very small, determining that the outliers do not meet the condition, and if the product of the slopes of the two straight lines is-1, determining that the outliers meet the condition of the characteristic contour points.
Preferably, the candidate feature point set is tested by a laser sensor, and the number of the class set points is one tenth of the total number of the laser points.
Preferably, the method for scanning the characteristic road sign by the laser pose comprises the following steps:
when the laser pose is near the pose of the characteristic road sign or the sudden change situation occurs in positioning, clustering the laser point cloud carved at the moment by taking the points at an angle of 0-90 degrees;
if the characteristic road signs exist, absolute pose query is carried out on the original road sign set, and point cloud matching is carried out according with conditions to obtain a pose transformation matrix;
if no characteristic road sign exists, the laser point within the range of 90-180 degrees is continuously searched, the processing mode is the same as the above, the search is not carried out until the characteristic road sign exists, meanwhile, the current characteristic road sign needs to be updated, the original road sign is deleted, and the stored characteristic information is updated.
Preferably, the pose correction strategy includes:
if the positioning is suddenly changed, the pose matched by the road signs is used as the current pose;
if the pose change is normal, recording the pose change amount of each landmark matching, if the pose change amount is overlarge, recording the current landmark mark as the state needing to be updated, and if the pose change amount of the next landmark matching is overlarge, recording the current landmark as the state needing to be updated;
and when the third landmark is matched, comparing the pose change amount before and after the third landmark, updating the poses of the landmarks and updating the poses simultaneously if the corrected pose difference value is within the threshold range of 1cm, deleting the first pose correction and not updating the poses if the pose difference values respectively corrected for three times are too large, and continuing to perform the steps.
Compared with the prior art, the invention has the beneficial effects that:
effective features in the laser frames are extracted to serve as the road signs, when an environment map is constructed, the road signs are fused for optimization of positioning, stable and reliable pose output can be obtained, robustness and accuracy of robot positioning can be improved by the method, positioning loss is not prone to occurring, and the method is suitable for variable environments such as indoor factories.
Drawings
Fig. 1 is a flow chart of a feature-based landmark positioning method of the present invention.
FIG. 2 is a schematic diagram of a characteristic profile of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
As shown in fig. 1, a method for improving the accuracy and stability of 2D-SLAM based on feature road marking includes the following steps:
step S101, in the process of building a map by a laser scanning environment, clustering laser points to generate a clustering point set, taking an environment with densely distributed clustering points as a stable environment, and performing composition by an open source algorithm factor, and in the process of composition, after scanning environment information by laser, clustering laser points, wherein the clustering points of stable environments such as wall surfaces and corner walls are abundant, and other environments are possibly changeable, so that the stable environment is preferentially selected as a characteristic road sign, the L-shaped characteristic of the corner wall is extracted, if the characteristic extraction rule is met, the clustering point set is stored and stored according to the laser angle sequence, the clustering processing is to cluster the laser points in the laser cloud, the threshold value between adjacent points in the same set is set to be 1cm, and the distance threshold value between different sets to be 5 m;
step S102, performing outlier calculation on laser clustering points in a stable environment, extracting a clustering point set which accords with a characteristic contour as a characteristic road sign, and storing data of the clustering point set into the characteristic road sign set, wherein the characteristic contour extraction method comprises the following steps:
a. performing two times of ransac fitting on the candidate feature point set with the same class set larger than a certain number of points;
b. first fitting, namely, if the distance value under the same set is increased and then decreased within a certain distance range, the feature contour points are obtained;
c. and performing secondary ransac fitting to calculate the number of outliers, if the number is too small, determining that the outliers do not meet the condition, verifying the number of dichotomous points again, performing linear fitting once for each part, if the difference between the slopes of the two straight lines is very small, determining that the outliers do not meet the condition, and if the product of the slopes of the two straight lines is-1, determining that the outliers meet the condition of the characteristic contour points. The profile shape of the extracted L-shaped corner feature is shown in figure 2.
Step S103, in the positioning process, scanning out characteristic road signs according to laser poses, inquiring absolute poses in a road sign set, matching laser point clouds after conditions are met to obtain a pose transformation matrix, updating current characteristic road signs in the characteristic road sign set, and deleting original road signs;
and step S104, correcting the pose in the constructed environment map according to the pose variation.
And the candidate characteristic point set is tested by a laser sensor, and the number of the class set points is one tenth of the total number of the laser points.
The method for scanning the characteristic road sign by the laser pose comprises the following steps:
when the laser pose is near the pose of the characteristic road sign or the sudden change situation occurs in positioning, clustering the laser point cloud carved at the moment by taking the points at an angle of 0-90 degrees;
if the characteristic road signs exist, absolute pose query is carried out on the original road sign set, and point cloud matching is carried out according with conditions to obtain a pose transformation matrix;
if no characteristic road sign exists, the laser point within the range of 90-180 degrees is continuously searched, the processing mode is the same as the above, the search is not carried out until the characteristic road sign exists, meanwhile, the current characteristic road sign needs to be updated, the original road sign is deleted, and the stored characteristic information is updated.
The pose correction strategy comprises:
if the positioning is suddenly changed, the pose matched by the road signs is used as the current pose;
if the pose change is normal, recording the pose change amount of each landmark matching, if the pose change amount is overlarge, recording the current landmark mark as the state needing to be updated, and if the pose change amount of the next landmark matching is overlarge, recording the current landmark as the state needing to be updated;
and when the third landmark is matched, comparing the pose change amount before and after the third landmark, updating the poses of the landmarks and updating the poses simultaneously if the corrected pose difference value is within the threshold range of 1cm, deleting the first pose correction and not updating the poses if the pose difference values respectively corrected for three times are too large, and continuing to perform the steps.
It will be evident to those skilled in the art that the invention is not limited to the details of the foregoing illustrative embodiments, and that the present invention may be embodied in other specific forms without departing from the spirit or essential attributes thereof. The present embodiments are therefore to be considered in all respects as illustrative and not restrictive, the scope of the invention being indicated by the appended claims rather than by the foregoing description, and all changes which come within the meaning and range of equivalency of the claims are therefore intended to be embraced therein. Any reference sign in a claim should not be construed as limiting the claim concerned.
Furthermore, it should be understood that although the present description refers to embodiments, not every embodiment may contain only a single embodiment, and such description is for clarity only, and those skilled in the art should integrate the description, and the embodiments may be combined as appropriate to form other embodiments understood by those skilled in the art.