Disclosure of Invention
The application provides a laser radar positioning method and a related device thereof, which can remove interference points so as to improve the positioning accuracy of the laser radar.
In order to achieve the above object, the present application provides a laser radar positioning method, including:
estimating the predicted pose of the laser radar;
determining first coordinates of each laser point in the current frame laser point cloud data based on the predicted pose and the obtained current frame laser point cloud data;
screening out laser points with poor matching degree based on the first coordinates to obtain a first laser point set;
matching the first laser point set with the grid map based on the predicted pose to obtain a second coordinate of the laser point in the first laser point set;
screening out laser points with poor matching degree based on the second coordinate to obtain a second laser point set;
and matching the second laser point set with the map to determine the final pose of the laser radar.
Screening out laser points with poor matching degree based on the second coordinate to obtain a second laser point set;
clustering laser points with similar distances in the first laser point set based on the second coordinate to obtain a plurality of classes;
screening out from each class a set of points of the laser spot that are greater than a first threshold from a second matching occupancy grid, wherein the second matching occupancy grid for each laser spot is the occupancy grid closest to each laser spot determined based on the second coordinates;
confirming the matching degree of the point concentration laser points and the second matching occupied grids, and screening out laser points with poor matching degree; and/or the presence of a gas in the gas,
and when the total number of the laser points of the point set is smaller than a second threshold value and the ratio of the total number of the laser points of the point set to the total number of the laser points in the class to which the point set belongs is smaller than a third threshold value, screening out all the laser points in the point set smaller than the third threshold value.
Wherein, confirm that the matching degree that laser point and second matching occupy the grid in the point set screens out the laser point that the matching degree is poor, include:
confirming the number of the laser points in the point set corresponding to the same second matching occupation grid, and screening out the laser points corresponding to the same second matching occupation grid when the number of the laser points in the point set exceeds a fourth threshold value and the distance between two laser points which are farthest away in the laser points corresponding to the same second matching occupation grid exceeds a fifth threshold value; and/or the presence of a gas in the gas,
taking the laser points and a plurality of adjacent laser points of the laser points in the point set in the class to which the point set belongs as a feature calculation point set, and calculating the geometric features of the geometric figure to which the laser points in the point set belong based on the feature calculation point set; and calculating a reference geometric characteristic based on the second matching occupation grids of all the laser points in the characteristic calculation point set, and screening out the laser points with low similarity degree based on the similarity degree of the geometric characteristic and the reference geometric characteristic.
The method for calculating the geometric characteristics of the geometric figure to which the laser points in the point set belong based on the characteristic calculation point set comprises the following steps: the normal vector and the curvature radius of the laser point are approximately solved by utilizing the eigenvalue and the eigenvector of the covariance of the distribution of the characteristic calculation point set;
computing a reference geometric feature based on a second matching occupancy grid of all laser points in the set of feature computation points, comprising: approximately solving a reference normal vector and a reference curvature radius of the laser point based on the eigenvalue and eigenvector of the covariance distributed by the second matching occupation of the center points of the grids in the feature calculation point set;
and screening out laser points with low similarity degree based on the similarity degree of the geometric features and the reference geometric features, wherein the screening out laser points comprises the following steps: when the included angle between the normal vector of the laser point and the reference direction quantity is larger than a sixth threshold value and smaller than a seventh threshold value, screening out the laser points which are larger than the sixth threshold value and smaller than the seventh threshold value;
and when the absolute value of the difference value between the curvature radius of the laser point and the reference curvature radius is larger than an eighth threshold value, screening out the laser points larger than the eighth threshold value.
Wherein screening out from each class a set of points of the laser spot larger than a first threshold from the first occupancy grid previously comprises: calculating a residual vector of the laser spot according to the second coordinate of the laser spot and the coordinate of the first occupied grid center point of the laser spot;
screening out from each class a set of points of the laser spot larger than a first threshold from the first occupancy grid, comprising: and screening out a point set of the laser points with the residual vector modular length larger than a first threshold value from each class, wherein the laser point residual vector modular length is calculated by the residual vector of the laser points.
And matching the second laser point set with a map to determine the final pose of the laser radar, wherein the method comprises the following steps: calculating a residual vector of the laser spot according to the second coordinate of the laser spot and the coordinate of the first occupied grid center point of the laser spot;
matching the second set of laser points to the map to determine a final pose of the lidar, previously comprising: and determining the weight coefficient of each laser point in the second laser point set, and determining the final pose of the laser radar according to the weight coefficient and the residual vector of each laser point in the second laser point set.
Wherein the weight coefficient of each laser spot in the second laser spot set is initially 1, and determining the weight coefficient of each laser spot in the second laser spot set includes:
calculating the direction angle of the residual vector of the laser point according to the residual vector of the laser point in the second laser point set;
clustering laser points with similar residual vector directions based on the residual vector direction angles to obtain a plurality of subsets;
and matching the subsets in the subset pairs with the opposite residual vector directions with the raster map one by one to confirm the matching scores of the subsets, and updating the weight coefficients of all the laser points in the subsets based on the matching scores.
Wherein, matching the subsets in the subset pairs with opposite residual vector directions with the grid map one by one, comprises:
confirming the sum of the residual vector moduli of all laser points in each subset of the subset pair with the opposite residual vector directions, and calculating the sum of the residual vector moduli of all the laser points in the second laser point set;
confirming whether the ratio of the sum of the modulus lengths of the residual vectors of all the subsets in the subset pair with the opposite residual vector direction to the sum of the modulus lengths of the residual vectors of the second laser point set is larger than a ninth threshold value;
and when the ratio is greater than a ninth threshold value and the ratio of the number of the laser points in all the subsets in the subset pairs with opposite residual vector directions to the total number of the laser points in the second laser point set exceeds a tenth threshold value, matching the subsets in the subset pairs with opposite residual vector directions with the grid map one by one.
Wherein, sieve out the laser point that the degree of matching is poor based on first coordinate, include:
taking the occupancy grid determined based on the first coordinates closest to each laser spot as a first matching occupancy grid for each laser spot;
laser spots that are greater than an eleventh threshold away from the first matching occupancy grid are screened out.
Wherein, estimating the predicted pose of the lidar comprises:
the predicted pose of the lidar is calculated based on other sensor information, or,
and estimating the linear velocity and the yaw angular velocity of the laser radar based on the final pose of the laser radar determined by the previous frames of laser point cloud data, and determining the predicted pose of the laser radar based on the linear velocity and the yaw angular velocity and the time interval between the current frame of laser point cloud data and the previous frame of laser point cloud data.
In order to achieve the above object, the present application provides a lidar positioning apparatus comprising a memory and a processor; the memory has stored therein a computer program for execution by the processor to perform the steps of the above method.
To achieve the above object, the present application provides a computer readable storage medium having a computer program stored thereon, which when executed by a processor, performs the steps of the above method.
The method comprises the following steps: firstly, determining a first coordinate of each laser point based on the predicted pose of the laser radar, determining the matching degree of the laser points and the grid map based on the first coordinate, and screening out the laser points with poor matching degree, so that the laser points with poor matching degree are screened out before matching, the matching precision can be improved, and the second coordinate of the laser points in the first laser point set is closer to a real coordinate; and after matching, laser points with poor matching degree are screened out based on the second coordinate, so that interference points can be further removed, and the positioning accuracy of the laser radar is improved.
Detailed Description
In order to make those skilled in the art better understand the technical solution of the present application, the lidar positioning method and the related apparatus provided by the present application will be described in further detail below with reference to the accompanying drawings and the detailed description.
Referring to fig. 1, fig. 1 is a schematic flow chart of an embodiment of a laser radar positioning method according to the present application. The laser radar positioning method of the embodiment includes the following steps.
S110: and estimating the predicted pose of the laser radar.
Firstly, in the laser radar positioning process, the predicted pose of the laser radar can be predicted first, so that some laser points with poor matching degree can be screened out based on the predicted pose.
The pose information of the laser radar includes position information and yaw angle information of the laser radar. The position information of the laser radar may refer to two-dimensional position information or three-dimensional position information of the laser radar. For example, the pose information of the lidar may be used
And (4) showing. The predicted pose, the accurate pose and the initial pose of the laser radar can be used as the predicted pose of the laser radar.
In an implementation, the predicted pose of the lidar may be calculated based on other sensor information. Other sensors may be IMU, odometer, etc.
In another implementation, a predicted pose of the lidar is determined based on an accurate pose of the lidar determined from the previous frames of laser point cloud data. Specifically, in the case where the pose information includes position information and yaw angle information of the laser radar, the linear velocity and yaw angle velocity of the laser radar may be estimated from the accurate pose of the laser radar determined from the previous frames of laser point cloud data, and the predicted pose of the laser radar may be determined based on the linear velocity and yaw angle velocity, and the time interval between the current frame of laser point cloud data and the previous frame of laser point cloud data.
Illustratively, the first two frames of laser point cloud data S are utilized
k-1And S
k-2Linear velocity of determined accurate pose estimation current time k of laser radar
And yaw rate
Using estimated linear velocity
And yaw rate
And calculating the time interval delta t to predict the predicted pose of the laser radar at the current moment:
wherein the content of the first and second substances,
and
respectively, linear velocity and yaw rate, (x)
k-1,y
k-1,ψ
k-1) Is the accurate pose of the laser radar determined by the previous frame of laser point cloud data
And the predicted pose of the laser radar of the current frame laser point cloud data.
Certainly, in order to obtain a more accurate pose of the laser radar through the laser radar positioning method, a frame of laser point cloud data is used before estimating the predicted pose of the laser radar, and the initial pose of the laser radar is solved by using a CSM (correlation matching) method and a branch and bound method according to a priori initial value range.
S120: and determining first coordinates of each laser point in the current frame laser point cloud data based on the predicted pose and the obtained current frame laser point cloud data.
Calculating current frame excitation under the condition of predicting poseLight spot cloud data SkEach laser point p ini(i ═ 1,2, …, N) coordinates in the map coordinate system, which were taken as the first coordinates for each laser spot.
S130: and screening out laser points with poor matching degree based on the first coordinate to obtain a first laser point set.
And determining the matching degree of each laser point and the grid map based on the first coordinate of each laser point, screening out the laser points with poor matching degree, and reserving the laser points with good matching degree to obtain a first laser point set.
It will be appreciated that there will typically be an occupancy grid on the grid map corresponding to the laser points in the laser point cloud data, so the first matching occupancy grid for each laser point may be determined based on the first coordinates of each laser point. Alternatively, a grid corresponding to the laser point on the grid map may be determined based on the first coordinates of the laser point, and then an occupied grid closest to the grid corresponding to the laser point may be used as the first matching occupied grid of the laser point. And the grid where the coordinates of the laser points on the grid map are located is the grid corresponding to the laser points on the grid map. The first coordinates of the laser spot and the second coordinates of the laser spot may be used as the coordinates of the laser spot.
The degree of matching between the laser spot and its first matching occupied grid may refer to the degree of positional similarity between the two and the degree of shape similarity between the two, and the like. Accordingly, a laser spot having a low degree of similarity in occupying grid position and/or shape with the first matching can be used as a laser spot having a poor degree of matching.
For example, the degree of match between a laser spot and its first matched occupied grid may be expressed in terms of the distance between the laser spot and its first matched occupied grid. Such that a laser spot occupying the grid with the first matching at a distance exceeding the eleventh threshold may be regarded as a laser spot having a poor degree of matching. It will be appreciated that the distance between the two grids may be calculated from the coordinates of the two grids. The coordinates of the grid may be expressed in coordinates of a fixed point of the grid. The fixed point of the grid can be any point in the grid, such as the center point of the grid, the top left vertex of the grid, the top right vertex of the grid and the like.
Of course, the distance between the laser spot and its first matching occupied grid can also be determined in other ways. For example, a distance map corresponding to the grid map is established before determining that the laser spot occupies the grid with its first match. In the distance map, the distance corresponding to the occupied grid is 0, and the distance corresponding to the unoccupied grid is the distance between the occupied grid closest to the unoccupied grid and the unoccupied grid. And then when the distance between the grids occupied by the laser spot and the first matching of the laser spot is determined, firstly determining the grids corresponding to the laser spot, and determining the distance corresponding to the grids corresponding to the laser spot based on the distance mapping map.
S140: and matching the first laser point set with the grid map based on the predicted pose to obtain a second coordinate of the laser point in the first laser point set.
And matching the first laser point set with the grid map by taking the predicted pose as a matching initial value to obtain a second coordinate of the laser point in the first laser point set. The matching method may be ICP, NDT, or a correlation matching method, etc.
S150: and screening out the laser points with poor matching degree based on the second coordinate to obtain a second laser point set.
And determining the matching degree of each laser point in the first laser point set and the grid map based on the second coordinate of each laser point, screening out the laser points with poor matching degree, and reserving the laser points with good matching degree in the first laser point set to obtain a second laser point set.
Alternatively, a second matching occupied grid of each laser spot may be determined based on the second coordinates of each laser spot, then the matching degree between each laser spot in the first set of laser spots and the second matching occupied grid thereof is determined, and laser spots with poor matching degree in the first set of laser spots are screened out to obtain a second set of laser spots.
The grid corresponding to the laser point on the grid map may be determined based on the second coordinate of the laser point, and then the occupied grid closest to the grid corresponding to the laser point may be used as the second matching occupied grid of the laser point.
In addition, the degree of matching between each laser spot and its second matching occupied grid may refer to the degree of positional similarity between the two and/or the degree of shape similarity between the two, and the like. Accordingly, a laser spot having a low degree of similarity in position and/or shape to the second occupied grid can be used as a laser spot having a poor degree of matching.
Further, a portion of the laser spot occupying the grid with a lower degree of positional similarity or shape similarity with the second matching may be regarded as a laser spot having a poor degree of matching. For example, a part of the laser points occupying the position similarity of the grid with the second matching is taken as the laser points having a poor matching degree. Based on this, as shown in fig. 2, step S150 may include the following steps.
S151: and clustering the laser points with similar distances in the first laser point set based on the second coordinate to obtain a plurality of classes.
Specifically, the distance between two laser points adjacent to each other in the position is calculated by using the second coordinates of the two laser points adjacent to each other in the position, and if the distance between the two laser points adjacent to each other in the position is smaller than a twelfth threshold, the two laser points adjacent to each other in the position belong to the same class; and if the distance between two laser points adjacent to each other is larger than the twelfth threshold, the two laser points adjacent to each other do not belong to the same class. And judging whether the distance between the two laser points adjacent to each other in the position is smaller than a twelfth threshold value or not in sequence, and classifying the two laser points adjacent to each other in the position smaller than the threshold value into the same class to obtain a plurality of classes.
Each laser point has a corresponding serial number, and two laser points adjacent to the serial number are generally adjacent in position, so that whether the distance between two laser points adjacent to the serial number is smaller than a twelfth threshold value or not can be directly judged, whether the distance between two laser points adjacent to the position is smaller than the twelfth threshold value or not is judged, and the calculation process is simplified.
S152: a set of points from each class is screened from the laser spot whose distance to the second match occupies a grid greater than a first threshold.
The distance between each laser point and the second matching occupied grid of each class can be calculated, and then laser points which are larger than a first threshold value from the second matching occupied grid are screened out from each class, so as to obtain a point set of the laser points with the distance larger than the first threshold value in each class.
It will be appreciated that the step of calculating the distance between the respective laser spot and its second matching occupied grid in each class may be performed before step S151.
In one implementation, the distance between the grid corresponding to each laser point in each class and the second matching occupied grid of each laser point can be directly calculated.
In another implementation, the residual vectors of the laser spots may be calculated from the second coordinates of the laser spots and the coordinates of the second matching occupied grid of laser spots, and then the modulo lengths of the residual vectors of the respective laser spots may be calculated. And the mode length of the residual vector of each laser point is the distance between each laser point and the second matching occupied grid of each laser point. It will be appreciated that the corresponding grid of laser points may be determined using the second coordinates of the laser points, and then the residual vector of each laser point may be calculated using the coordinates of the grid corresponding to each laser point and the coordinates of the grid corresponding to each laser point occupied by the second matching of each laser point in each class. The specific calculation formula of the residual vector of each laser spot is as follows:
wherein the content of the first and second substances,
coordinates that refer to a second match of individual laser points occupying the grid;
the coordinates of grids corresponding to each laser point are referred to;
refers to the residual vector of each laser spot.
S153: and when the total number of the laser points of the point set is smaller than a second threshold value and the ratio of the total number of the laser points of the point set to the total number of the laser points in the class to which the point set belongs is smaller than a third threshold value, screening out all the laser points in the point set smaller than the third threshold value.
Discrete interference points in the class are eliminated by step S153.
S154: and confirming the matching degree of the laser points in the point set and the second matching occupying grid, and screening out the laser points with poor matching degree.
And determining the matching degree of each laser point occupying the grid by the second matching of each laser point in the point set of the laser points with the distance of each class larger than the first threshold value, and screening out the laser points with poor matching degree.
In one implementation, if the number of laser points in the point set corresponding to the same second matching occupancy grid is greater than a fourth threshold, it may be said that at least part of the laser points have an occupancy grid matching error. Wherein the fourth threshold value can be adjusted according to the laser scanning precision. Alternatively, in the case where the fourth threshold is exceeded, the multiple laser points in the set of points matching the same matching occupancy grid may all be deleted. Alternatively, in the case where the fourth threshold is exceeded, the laser spot whose occupancy grid matches erroneously may be further selected from all the laser spots in the spot set that match the same occupancy grid, and then the laser spot whose occupancy grid matches erroneously may be screened out. Still alternatively, in the case where the fourth threshold is exceeded, it may be further confirmed whether the distance of two laser dots farthest from among all the laser dots in the dot set matching the same matching occupancy grid exceeds a fifth threshold, and if the distance exceeds the fifth threshold, the laser dots corresponding to the same second matching occupancy grid are screened out. It will be appreciated that the maximum and minimum values may be taken from the sequence numbers of all laser points in the set of points where the same match occupies a grid match; and taking the distance between the laser point corresponding to the maximum value of the serial number and the laser point corresponding to the minimum value of the serial number as the distance between the two laser points which are farthest away from all the laser points in the point set matched with the same matching occupation grid.
In another implementation, the shape similarity between the respective laser spot and its second matching occupancy grid in each spot set can be further confirmed, and laser spots with poor shape similarity can be screened out. In particular, the similarity of the shape of the laser spot and its second matching occupied grid can be confirmed by comparing the geometrical features of the laser spot and its second matching occupied grid: if the difference between the geometrical characteristics of the laser point and the geometrical characteristics of the grid occupied by the second matching of the laser point is larger, the shape similarity of the grid occupied by the laser point and the second matching of the laser point is poor; otherwise, the shape similarity is good. Where the geometric feature may be a radius of curvature, a normal vector, or other value.
Specifically, the step of comparing the geometrical characteristics of the laser spot and the second matching occupied grid, confirming the matching degree of the laser spot and the second matching occupied grid, and screening out the laser spots with poor matching degree may include: taking the laser points and a plurality of adjacent laser points of the laser points in the point set in the class to which the point set belongs as a feature calculation point set, and calculating the geometric features of the geometric figure to which the laser points in the point set belong based on the feature calculation point set; and calculating a reference geometric characteristic based on the second matching occupation grids of all the laser points in the characteristic calculation point set, and screening out the laser points with low similarity degree based on the similarity degree of the geometric characteristic and the reference geometric characteristic. Since the sequence number adjacency basically means that the laser point positions are adjacent, a plurality of points of the laser point adjacent to the sequence number in the class to which the point set belongs can be regarded as a plurality of laser points of the laser point adjacent in the class to which the point set belongs. For example, a class includes a number of points with serial numbers 2, 5, 6, 7, 8, 12, 15, 19, 22, and 26, respectively, where 7 is a laser point whose distance is greater than a first threshold, and when calculating the geometric feature of 7, 5, 6, 7, 8, and 12 may be used as a feature calculation point set, the geometric feature of 7 may be calculated using the feature calculation point set, and the reference geometric feature may be calculated using the coordinates of the second matching occupied grid of 5, 6, 7, 8, and 12, respectively.
Further, when the geometric features of the shape similarity are judged to be normal vectors and curvature vectors, the calculating of the geometric features of the geometric figure to which the laser point belongs in the point set based on the feature calculation point set may include: and (4) approximately solving the normal vector and the curvature radius of the laser point by utilizing the eigenvalue and the eigenvector of the covariance of the distribution of the characteristic calculation point set. Wherein computing the reference geometric feature based on the second matching occupancy grid of all laser points in the set of feature computation points may include: and approximately solving a reference normal vector and a reference curvature radius of the laser point based on the eigenvalue and eigenvector of the covariance of the distribution of the central points of the second matching occupation grids of all the laser points in the feature calculation point set. When the included angle between the normal vector of the laser point and the reference normal vector is larger than a sixth threshold and smaller than a seventh threshold, the laser points larger than the sixth threshold and smaller than the seventh threshold are screened out; and/or when the absolute value of the difference value between the curvature radius of the laser point and the reference curvature radius is larger than an eighth threshold value, screening out the laser points larger than the eighth threshold value. The sixth threshold, the seventh threshold and the eighth threshold can be selected according to actual conditions. Preferably, the sixth threshold is close to 0 °, and may take the value 5 °. Preferably, the seventh threshold is close to 180 °, and may take 175 °.
The execution sequence of step S154 and step S153 is not limited. For example, step S154 may be performed before step S153, or simultaneously with step S153. In addition, in other embodiments, only step S154 may be performed, or only step S153 may be performed.
S160: and matching the second laser point set with the map to determine the accurate pose of the laser radar.
In an implementation mode, the second laser point set obtained through twice screening can be directly matched with a map for calculation so as to obtain the accurate pose of the laser radar.
In another implementation manner, the weight coefficient of each laser point can be determined based on the matching score of each laser point in the second laser point set and the grid map, then the second laser point set and the grid map are subjected to weighted matching based on the weight coefficient of each laser point, and the accurate pose of the laser radar is obtained through matching. And the residual vector corresponding to each laser point in the target function of the weighted matching process needs to be multiplied by the weight coefficient of the laser point. In addition, the method of weighted matching may include matching methods such as ICP, NDT, CSM, and the like.
Further, all laser points may be directly matched with the grid map to determine matching scores of the respective laser points with the grid map.
Or all laser points of the second laser point set can be divided into a plurality of subsets according to the residual vector, the subsets of the weight coefficients needing to be confirmed are matched with the grid map one by one to obtain matching scores of the subsets of the weight coefficients needing to be confirmed, and then the weight coefficients of all the laser points in the subsets are determined based on the matching scores of the subsets.
It can be understood that, in order to avoid the mutual influence of the two groups of laser points with opposite residual vector directions when the laser points are matched with the raster map, the two groups of laser points with opposite residual vector directions can be respectively classified into different subsets, and then the two groups of laser points with opposite residual vector directions are respectively matched with the map, so as to reduce the matching error and obtain an accurate matching score.
Preferably, all laser points of the second set of laser points may be divided into a plurality of subsets according to the direction of the residual vector, and the directions of the residual vectors of all laser points in each subset are the same or similar, so that two groups of laser points with opposite directions of the residual vectors necessarily belong to different subsets.
Illustratively, the method of dividing all laser points of the second set of laser points into a plurality of subsets may comprise: calculating the direction angle of the residual vector of the laser point according to the residual vector of the laser point in the second laser point set; and clustering the laser points with similar residual vector directions based on the residual vector direction angles to obtain a plurality of subsets. Wherein, the direction angle of the residual vector
The calculation formula of (a) is as follows:
wherein the content of the first and second substances,
is the residual vector of the laser spot;
is the residual vector direction angle of the laser spot.
It can be understood that the laser points with the residual vector direction angles in the same angle interval may be laser points with similar residual vector directions, and the residual vector direction angles of all the laser points in the second set of laser points are in the combination of all the angle intervals, and the different angle intervals do not overlap with each other. The difference between the upper limit value of the angle interval and the lower limit value of the angle interval may be an angle window threshold. In addition, the angle window thresholds for different angle intervals may be the same or different.
It should be noted that not all subsets need to determine the weighting coefficients based on the matching scores, that is, the weighting coefficients of all laser points in some subsets may be directly fixed values, for example, 1, although the value of the fixed value is not limited thereto.
Here, the subset having a large influence on the matching result may be determined as the weight coefficient based on the matching score, and the weight coefficients of all laser points in the subset having a small influence on the matching result may be set as fixed values. It will be appreciated that there are subsets where the number of laser spots occupies a larger grid distance from its second matching, and the number of laser spots is large, which may be a subset that has a large impact on the matching result. The sum of the modular lengths of all the residual vectors of laser spots in the subset can be compared with the sum of the modular lengths of all the residual vectors of laser spots in the second set of laser spots to determine whether there are laser spots in the subset that are further away from their second matching occupancy grid. Illustratively, if the ratio of the sum of the residual vector modulo lengths of the subset to the sum of the residual vector modulo lengths of the second set of laser points is greater than the ninth threshold, this indicates that there are several laser points in the subset that are further away from the second matching occupancy grid, and if the total number of laser points in the subset exceeds the tenth threshold, this subset is the subset that has a large influence on the matching result.
In other implementations, it may be determined whether it is necessary to determine the weight coefficients based on the matching scores for all subsets within a subset pair in units of determination of the subset pair. Wherein a subset pair comprises two subsets with opposite residual vector directions. For example, when both subsets of a subset pair are subsets that have a large influence on the matching result, all subsets of the subset pair need to be validated for weight coefficients based on the matching scores; when at least one of the two subsets of the subset pair is a subset having a small influence on the matching result, the weight coefficients of all laser points of all subsets within the subset pair are set to a fixed value without confirming the weight coefficients based on the matching scores for all subsets of the subset pair.
To better illustrate the steps of determining the weighting factors of all laser points in the second set of laser points in the positioning method of the present application, the following specific embodiments are provided for illustrative purposes:
counting the residual vectors of each laser point in the second laser point set, clustering the laser points with similar residual vector directions, and extracting a laser point set pair with opposite residual vector directions, wherein the method comprises the following steps:
1) calculating the direction angle of the matched residual vector:
traversing the second laser point set, and calculating the angle and the modular length of the residual vector direction of each laser point; wherein the direction angle of the vector pointing to the positive direction of the X axis of the map coordinate system is 0, and the anticlockwise direction is positive; matching residual vector direction angle of ith laser point
2) Clustering of matching residual vectors:
setting an angle window threshold β to be 5 degrees × i (i is not less than 1 and not more than 4), starting from-180 degrees, clustering in a β angle range (such as [ -180 degrees, β -180 degrees), [ β -180 degrees, 2 β -180 degrees °) …), clustering according to the matching residual vector direction angles of the laser points to obtain a plurality of subsets which are recorded as
3) Screening out laser point subset pairs which need to confirm the weight coefficient based on the matching score and have opposite residual vector directions:
wherein, in
When the temperature of the water is higher than the set temperature,
and
pairs of subsets of laser points with opposite residual vector directions.
Statistics of
And the sum of the modulo lengths of the residual vectors in (1) is recorded as
The sum of the modulus lengths of the residual vectors of all laser points is recorded as
If it is not
The laser point subset pair (marked as matching score-based) requiring the confirmation of the weight coefficient is satisfied
And
):
A. setting a proportional threshold
B. Laser spot set
Modulo sum of residual vector of
C. Setting a proportional threshold
Total number of laser spots of
Laser spot set
And
number of points (0)<α<1) All amounts are greater than
4) Calculating the weight coefficient of each laser point
A. Initializing the weight coefficients for each laser spot
Is 1.
B. Will need to confirm the weight coefficients based on the matching scores
Of all pairs of subsets of laser spots
And
respectively matching with map by CSM (correlation matching) method, and matching with lower-grade point set laser point weight coefficient
Set to α (0)<α<1) Wherein α is positively correlated with the match score.
In the embodiment, the first coordinate of each laser point is determined based on the predicted pose of the laser radar, the matching degree of the laser points and the grid map is determined based on the first coordinate, and the laser points with poor matching degree are screened out, so that the laser points with poor matching degree are screened out before matching, the matching precision can be improved, and the second coordinate of the laser points in the first laser point set is closer to the real coordinate; and after matching, laser points with poor matching degree are screened out based on the second coordinate, so that interference points can be further removed, and the accuracy of laser radar positioning is improved.
It can be understood that the laser radar positioning method can be applied to robot positioning, and because the laser radar is generally installed on the robot when the robot is positioned by the laser radar, the accurate pose of the laser radar determined based on the laser radar positioning method can be used as the accurate pose of the robot.
Referring to fig. 3, fig. 3 is a schematic structural diagram of an embodiment of a lidar positioning apparatus according to the present application. The laser radar positioning device 10 comprises a processor 12 and a memory 11; memory 11 is configured to store program instructions for implementing the laser radar location methods described above, and processor 12 is configured to execute the program instructions stored by memory 11.
The logic processes of the above-mentioned lidar positioning method are presented as a computer program, which may be stored in a computer-readable storage medium if it is sold or used as a stand-alone software product, and thus the present application proposes a computer-readable storage medium. Referring to fig. 4, fig. 4 is a schematic structural diagram of an embodiment of a computer-readable storage medium 20 of the present application, in which a computer program 21 is stored, and when the computer program is executed by a processor, the steps in the laser radar positioning method are implemented.
The computer-readable storage medium 20 may be a medium that can store a computer program, such as a usb disk, a removable hard disk, a Read-only Memory (ROM), a Random Access Memory (RAM), a magnetic disk or an optical disk, or may be a server that stores the computer program, and the server can send the stored computer program to another device for running or can run the stored computer program by itself. The computer readable storage medium 20 may be a combination of a plurality of entities from a physical point of view, for example, a plurality of servers, a server plus a memory, or a memory plus a removable hard disk.
The above embodiments are merely examples and are not intended to limit the scope of the present disclosure, and all modifications, equivalents, and flow charts using the contents of the specification and drawings of the present disclosure or those directly or indirectly applied to other related technical fields are intended to be included in the scope of the present disclosure.