A Map Compression Method Based on the Sensitivity Analysis of Positioning Error

Abstract

The present invention discloses a map compression method based on positioning error sensitivity analysis. The method includes: obtaining a dense point cloud map to be compressed, performing lidar simulation sampling with multiple poses in the feasible region of this map, then calculating and analyzing the sensitivity of the sampled map points to positioning errors, using the calculated results to score the map points in six degrees of freedom, then using the normalized product method to fuse the six-dimensional scores into a one-dimensional positioning contribution degree, and finally using a multi-resolution map compression unit and an adaptive point retention method to retain and delete map points in combination with the size of the contribution degree. The present invention realizes that when the compression rate is extremely low, the compressed map can still maintain a high-precision positioning effect.

Description

Technical Field

[0001] The present invention relates to the field of robot navigation and positioning (computer technology / Simultaneous Localization and Mapping (SLAM) technology), and particularly relates to a map compression method for positioning tasks. Background Art

[0002] Maps play a very important role in the positioning of mobile robots, because they can serve as global constraints to eliminate the cumulative errors of positioning methods such as odometers and Simultaneous Localization and Mapping (SLAM), and improve the accuracy and robustness of positioning. In recent years, in the field of autonomous driving, positioning based on prior maps has received increasing attention.

[0003] Obviously, the quality of the map will greatly affect the positioning performance. Specifically, it is reflected in the following two aspects: (1) The size of the map: The larger the map, the slower the data transmission, the greater the storage space consumption, and the worse the positioning real-time performance; (2) The proportion of positioning effective information in the map: The higher the proportion of information effective for positioning, the higher the positioning accuracy.

[0004] Currently, common map forms in the field of mobile robots include: point cloud maps, patch maps, semantic maps, etc. Although point cloud maps have a large amount of data, they retain the most basic and original scene structure information. In addition, due to the high measurement accuracy of lidar, the point cloud maps constructed by it are most commonly used in positioning tasks. However, there are still huge challenges in applying point cloud maps to mobile robots with limited storage and computing resources.

[0005] To solve this problem, many point cloud compression methods have been proposed in the fields of computer vision and robotics. Most of them focus on the expression, reproduction, and transmission efficiency of point clouds, without considering the screening of effective point cloud information for positioning.

[0006] Some methods consider the observability of map points (the frequency that can be observed during the positioning process) and the characteristics of map points (including the intensity information, normal vector, density, relative position with the sensor to be positioned, etc.) of map points. Among them, the characteristics of map points are used as constraint conditions in the positioning task after map compression. Therefore, only the observability of map points is used as the basis for map compression, that is, the map points with more observation times on a trajectory are retained. This results in the compression result of the entire map being strongly correlated with the observation trajectory, and the fact that map points have more observation times does not guarantee an improvement in positioning accuracy. In practical applications, since the running trajectory of a mobile robot is a feasible area, this method strongly correlated with a single trajectory has great limitations. Therefore, in addition to observability, on the one hand, more explicit indicators for improving positioning accuracy need to be proposed, and on the other hand, the actual driving trajectory and scene need to be considered. Summary of the Invention

[0007] The object of the present invention is to provide a map compression method based on positioning error sensitivity analysis for the deficiencies of the prior art. It uses the positioning contribution degree of map points as positioning effective information, and at the same time takes into account the actual feasible area and scene structure information, ensuring the effectiveness of the compressed map for the positioning task and being able to maintain a high level of positioning accuracy at an extremely low compression rate.

[0008] The object of the present invention is achieved through the following technical solutions: A map compression method based on positioning error sensitivity analysis, the method comprising the following steps:

[0009] S1: Obtain a dense point cloud map to be compressed, including line feature points and surface feature points;

[0010] S2: Perform multi-pose lidar simulation sampling in the feasible area of the map obtained in S1. Specifically: According to the lidar simulation sampling model, simulate the point cloud generation method of the lidar. For a given pose relative to the map, generate a frame of simulated sampling point cloud in the dense point cloud map; In the map, there is a feasible area for the mobile robot to move, and a series of sampling pose points are generated according to the feasible area;

[0011] S3: Calculate and analyze the sensitivity of the sampled map points to the positioning error, and use the calculated results to score the map points in six degrees of freedom. Specifically: Calculate the derivative of the error of estimating the current lidar pose with this map point with respect to this pose to obtain the positioning error sensitivity matrix. That is, the positioning error sensitivity calculation formula is Δe ij = JΔT ij , where J is the positioning error sensitivity matrix, e ij is the error of estimating the current lidar sampling pose P j using the map point p i , T ij is the pose transformation matrix estimated for the current lidar pose P j using the map point p i , and the positioning error sensitivity matrix is used as the six-degree-of-freedom score for a single simulation sampling of the map point;

[0012] S4: Use the normalized product method to fuse the six-dimensional scores into a one-dimensional positioning contribution degree. Specifically: Each degree of freedom of a single map point has multiple sampling scores. Through statistical analysis, the distribution of these score scores approximately follows a Gaussian distribution; Use the normalized product of the Gaussian probability density function to fuse the score scores of the six degrees of freedom into a one-dimensional score score, that is, the positioning contribution degree;

[0013] S5: Use a multi - resolution map compression unit and an adaptive point - retaining method to retain and delete map points according to the contribution degree. Specifically: divide the original map into grid cells of uniform size, i.e., compression units, screen the contribution degrees of map points in each compression unit, and use an adaptive method to retain map points with high contribution degrees.

[0014] Furthermore, the lidar simulation sampling model is used to simulate the sampling of the lidar in the map scene; based on the range - azimuth - elevation model RAE, for a pose P i of a multi - beam lidar, assume that N beams are emitted from the center of the lidar, i.e., the origin of the lidar coordinate system F, and the horizontal angular resolution is θ h ; during scanning, the N beams rotate around the z - axis of F for one week, and each beam will obtain 2π / θ h scanning points. Then, the scan frame S i corresponding to the pose P i will consist of N·2π / θ h scanning points; equivalently, the lidar simulation sampling model is regarded as N·2π / θ i beams emitted from the point P h ; add a field - of - view angle to each ray in the vertical direction to form N intervals; combined with the horizontal angular resolution, i.e., 2π / θ h intervals are formed in the horizontal direction; thus, the point - cloud map space based on the current sampling pose point is divided into N·2π / θ h intervals. Only select the point closest to the origin of the lidar coordinate system F from several points in each interval as the map point sampled by the simulation in the current interval, and form a scan frame at one pose of the current sampling position point.

[0015] Furthermore, generating a series of sampling pose points according to the feasible region specifically means that in actual use, the pose of the lidar limits the random sampling range of the lidar in the roll and pitch directions; at different poses of the lidar (changing the roll angle and pitch angle), multiple simulated sampling lidar scans at different poses of a single sampling position are obtained; then, plan a feasible region in the lidar point - cloud map and divide the feasible region into different sampling grids; the lidar will sample at the center of the grid in different poses; the specific grid size can be adjusted according to the scene.

[0016] Furthermore, calculate the 6 - degree - of - freedom sensitivity of a map point to the positioning error. This 6 - dimensional vector is used as the 6 - degree - of - freedom score for a single sampling of this map point. First, the sensitivity of the positioning error is given by the formula Δe ij = JΔT ijCalculation: The calculated Jacobian matrix J is a 6-dimensional matrix, serving as the 6-degree-of-freedom score for a single sampling of the map point. Here, the positioning error of the line feature point is given by the distance from the point to the line, and the positioning error of the plane feature point is given by the distance from the point to the plane. Therefore, the specific calculation formulas for the positioning error sensitivities of the two are different, and thus they need to be calculated and scored separately.

[0017] The positioning error sensitivity matrix J of the line feature point l The calculation formula is as follows:

[0018]

[0019] Where n lj is the normal vector of the line segment l corresponding to the line feature point p lj , is the skew-symmetric matrix of n lj , and the superscript T represents the matrix transpose. I 3 is the 3-order identity matrix;

[0020] The positioning error sensitivity J of the plane feature point s The calculation formula is as follows:

[0021]

[0022] Where n sj is the normal vector of the plane s corresponding to the plane feature point p sj , and the superscript T represents the matrix transpose.

[0023] Furthermore, the score for fusing the 6 degrees of freedom is a 1-dimensional score. First, the skewness formula (3) and kurtosis formula (4) are used to perform a normality test on the scoring distribution of different degrees of freedom of the map point;

[0024]

[0025]

[0026] Where x is the random variable, μ is the average value of x, σ is the covariance of x, E is the expectation, skewness represents skewness, and kurtosis represents kurtosis; it is tested that the scoring distribution is approximately a Gaussian distribution; then, the Gaussian probability density function's normalization product is used to fuse the 6-degree-of-freedom scores of the map point into a 1-dimensional contribution C, and the fusion formula is as follows:

[0027]

[0028] Where μ k and σ k are respectively the mean and variance of multiple scores of different degrees of freedom k.

[0029] Further, in each compression unit, the contribution degree of map points is screened. The original map is divided into grids of uniform size, which are called compression units, and the compression of map points is performed in each unit. When the number of map points in a compression unit is too small to provide enough map points to meet the given compression ratio, this compression unit is merged with adjacent compression units until the required number of map points is reached.

[0030] Further, the high-contribution map points are retained in an adaptive manner. First, when the high-contribution (the contribution degree value exceeds the set threshold) map points in the compression unit are too concentrated, random sampling is performed in this compression unit to make the distribution of the compressed map points relatively uniform. Then, to increase the compressibility of the point cloud map, while ensuring that the compression ratio of the overall map points remains unchanged, the number ratio of line feature points and surface feature points in the compressed map is appropriately increased.

[0031] Compared with the prior art, the beneficial effects of the present invention are as follows: The present invention constructs a lidar simulation sampling model to perform multiple samplings at different poses within the feasible region, realizing multi-trajectory positioning in the map compressed for one trajectory; uses the sensitivity analysis of positioning error to express the positioning information of map points, effectively recording the positioning contribution information of map points; uses the normalized product method to fuse the multiple scores of the 6 degrees of freedom of map points into a 1-dimensional contribution degree, theoretically ensuring a good positioning effect of the compressed map; uses multi-resolution map compression units and an adaptive point-retaining method, making the distribution of the compressed map points as uniform as possible on the premise of ensuring the positioning contribution degree, and further improving the positioning accuracy. Brief Description of the Drawings

[0032] Figure 1 It is the overall flowchart of the map compression method of the present invention. Detailed Embodiment

[0033] The embodiment of the present invention provides a map compression method based on the sensitivity analysis of positioning error, which is used to reduce the size of the positioning map of a mobile robot and can achieve a high-precision positioning effect at an extremely low compression ratio.

[0034] In order to enable those skilled in the art to better understand the solution of the present invention, the technical solution of the present invention will be clearly and completely described below with reference to the drawings of the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0035] The present invention first needs to obtain a dense point cloud map to be compressed, which can be obtained by a multi-sensor fusion SLAM algorithm. During the process, a line feature point map and a surface feature point map can be obtained respectively.

[0036] Figure 1 is the overall flowchart of a map compression method based on positioning error sensitivity analysis provided by the present invention. Referring to Figure 1 , the following four steps are performed on the obtained point cloud map to be compressed: (1) LiDAR simulation sampling in the feasible region of the map to be compressed; (2) Map point scoring based on positioning error sensitivity analysis; (3) Calculation of the contribution degree of map points by fusing 6-degree-of-freedom scoring; (4) Retaining and deleting map points using a multi-resolution map compression unit and an adaptive method.

[0037] In step (1), a LiDAR simulation sampling model is constructed to simulate the sampling of LiDAR in the map scene. Based on the range-azimuth-elevation model RAE, for a multi-beam LiDAR with pose P i , assuming that N beams are emitted from the center of the LiDAR, i.e., the origin of the LiDAR coordinate system F, and the horizontal angular resolution is θ h ; during scanning, the N beams rotate around the z-axis of F for one week, and each beam will obtain 2π / θ h scanning points. Then, the scan frame S i corresponding to the pose P i will be composed of N·2π / θ h scanning points; equivalently, the LiDAR simulation sampling model is regarded as N·2π / θ i beams emitted from point P h ; in the vertical direction, a field of view angle is added to each ray to form N intervals; combined with the horizontal angular resolution, i.e., 2π / θ h intervals are formed in the horizontal direction; thus far, the point cloud map space based on the current sampling pose point is divided into N·2π / θ h intervals, and only the point closest to the origin of the LiDAR coordinate system F among several points in each interval is selected as the map point sampled by the simulation in the current interval, forming a scan frame at one pose of the current sampling position point.

[0038] According to the attitude of the lidar during actual use, the random sampling ranges in the roll and pitch directions of the lidar are restricted. At different attitudes of the sensor (i.e., changing roll / pitch), several simulated lidar scan frames of a single sampling point at different attitudes are obtained. At the same time, a feasible area is planned in the map, and several sampling grids are divided in the feasible area as sampling position points (such as 5m×5m×5m, and the specific grid size can be adjusted according to the scene). Lidar simulation sampling at different attitudes is carried out at each sampling position point. Furthermore, possible lidar simulation sampling map points within the feasible area in the map space are obtained.

[0039] In step (2) above, a positioning error sensitivity analysis is performed for each map point in the lidar simulation sampling. For map point p j Estimate the current sampling pose P of the lidar i The error is e ij , then the positioning error sensitivity calculation formula is:

[0040] Δe ij = JΔT ij #(1)

[0041] Where J is the Jacobian matrix, that is, the positioning error sensitivity matrix, and T ij Is the pose transformation matrix estimated for the current lidar pose P using map point p j . Use the 6-degree-of-freedom positioning error sensitivity matrix J as the score S i Of map point p j For the lidar sampling pose P i . The positioning error sensitivity matrix J ij Of the line feature point is calculated as follows: l The calculation formula is as follows:

[0042]

[0043] Where n lj Is the normal vector of the line segment l corresponding to the line feature point p lj , Is the skew-symmetric matrix of n lj , and the superscript T represents the matrix transpose. I 3 Is the 3rd-order identity matrix.

[0044] The positioning error sensitivity matrix J s Of the plane feature point is calculated as follows:

[0045]

[0046] Where n sj Is the normal vector of the plane s corresponding to the plane feature point p sj , and the superscript T represents the matrix transpose.

[0047] So far, the 6-DOF score of the map points can be obtained.

[0048] In step (3), the map points that can be observed in the feasible area of the map will obtain multiple 6-DOF scores. First, analyze the score distribution of different degrees of freedom of the map points, and use the skewness formula (4) and kurtosis formula (5) to perform a normality test on the score distribution of different degrees of freedom of the map points.

[0049]

[0050]

[0051] Where x is the random variable, μ is the mean of x, σ is the covariance of x, E is the expectation, skewness represents skewness, and kurtosis represents kurtosis. It is tested that the score distribution is approximately a Gaussian distribution. Then, use the normalized product of the Gaussian probability density function to fuse the 6-DOF scores of the map points into a 1D contribution C. The fusion formula is as follows:

[0052]

[0053] Where μ k and σ k are the mean and variance of the multiple scores of different degrees of freedom k respectively.

[0054] In step (4), the original map is divided into grid cells of uniform size, called map compression units. Map points are screened in each map compression unit. When the number of map points in a compression unit is too sparse to provide enough map points to meet the given compression rate, this compression unit is merged with adjacent compression units until the required number of map points is met, and finally a multi-resolution map compression unit is formed. During the process of map compression in the multi-resolution map compression unit, the contribution distribution of the map points in each map compression unit is analyzed. When the kurtosis of the compression unit is too high, that is, the points with high positioning contribution are too concentrated, then random sampling is performed on this compression unit. Finally, to increase the compressibility of the point cloud map, while ensuring that the compression rate of the overall map points remains unchanged, the proportion of the number of line feature points and surface feature points in the compressed map is appropriately increased.

[0055] After compressing the point cloud map using a map compression method based on positioning error sensitivity analysis proposed by the present invention, the results of lidar positioning of multiple trajectories in this compressed map are shown in Table 1. Among them, the voxel downsampling and random downsampling methods commonly used in point cloud map compression are compared. The results are judged using the absolute trajectory error (unit: meter), and the smaller the error, the higher the positioning accuracy. It can be seen from Table 1 that when the map compression ratios are 0.2% and 0.1%, compared with other methods, the present invention can obtain high-precision positioning results.

[0056] Table 1

[0057]

[0058] The above embodiments are only preferred and feasible embodiments of the present invention, which are used to illustrate the technical solutions of the present invention and are not intended to limit the protection scope of the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, it is still possible to modify the technical solutions recorded in the foregoing embodiments or perform equivalent substitution on some of the technical features without departing from the spirit and scope of the claims and their equivalents. Therefore, these modifications or substitutions are all within the protection scope of this technical solution.

Claims

1. A map compression method based on positioning error sensitivity analysis, characterized in that, it includes the following steps: S1: Obtain a dense point cloud map to be compressed, including line feature points and surface feature points; S2: Perform lidar simulation sampling with multiple poses in the feasible area of the map obtained in S1. Specifically: According to the lidar simulation sampling model, simulate the point cloud generation method of the lidar. For a given pose relative to the map, generate a frame of simulated sampling point cloud in the dense point cloud map; there is a feasible area in the map for the mobile robot to move. Generate a series of sampling pose points according to the feasible area; S3: Calculate and analyze the sensitivity of the sampled map points to the positioning error, and use the calculation results to score the map points with 6 degrees of freedom. Specifically, calculate the derivative of the error of estimating the current lidar posture with this map point to this posture, and obtain the positioning error sensitivity matrix, that is, the positioning error sensitivity calculation formula is Δe ij =JΔT ij , where J is the positioning error sensitivity matrix, e ij is to use the map point p j Estimate the current sampling pose P of the lidar i The error, T ij is to use the map point p j For the current LiDAR pose P i The estimated pose transformation matrix and the positioning error sensitivity matrix are used as the 6-DOF score of a single simulation sampling of the map points; S4: Use the normalized product method to fuse 6D scores into a 1D positioning contribution. Specifically: A single degree of freedom of a single map point has multiple sampling scores. Through statistical analysis, the distribution of these score scores approximately follows a Gaussian distribution; Use the normalized product of the Gaussian probability density function to fuse the score scores of 6 degrees of freedom into a 1D score score, that is, the positioning contribution. Specifically: First, use the skewness formula (1) and kurtosis formula (2) to perform a normality test on the score distribution of different degrees of freedom of the map point; where x is a random variable, μ is the mean of x, σ is the covariance of x, E is the expectation, skewness represents skewness, and kurtosis represents kurtosis; Test that the score distribution is approximately Gaussian; Then use the normalized product of the Gaussian probability density function to fuse the 6-degree-of-freedom scores of the map point into a 1D contribution C. The fusion formula is as follows: where μ k and σ k are the mean and variance of multiple scores with different degrees of freedom k, respectively; S5: Use a multi-resolution map compression unit and an adaptive point retention method to retain and delete map points according to the size of the contribution. Specifically: Divide the original map into grid cells of uniform size, that is, compression units. Screen the size of the contribution of map points in each compression unit, and use an adaptive method to retain map points with high contribution.

2. The map compression method based on positioning error sensitivity analysis according to claim 1, characterized in that, the lidar simulation sampling model is used to simulate the sampling of the lidar in the map scene; Based on the distance-azimuth-pitch model RAE, for the pose P i of the multi-beam lidar, assume that N beams are emitted from the center of the lidar, i.e., the origin of the lidar coordinate system F, and the horizontal angular resolution is θ h ; during scanning, the N beams rotate around the z-axis of F for one week, and each beam will obtain 2π / θ h scanning points. Then, the scanning frame S i corresponding to the pose P i will be composed of N·2π / θ h scanning points; equivalently, the lidar simulation sampling model is regarded as N·2π / θ i beams emitted from the point P h ; add the field of view angle to each ray in the vertical direction to form N intervals; combined with the horizontal angular resolution, that is, 2π / θ h intervals are formed in the horizontal direction; so far, the point cloud map space based on the current sampling pose point is divided into N·2π / θ h intervals. Only the point closest to the origin of the lidar coordinate system F is selected from several points in each interval as the map point sampled by the simulation of the current interval, and a scanning frame at one pose of the current sampling position point is formed.

3. The map compression method based on positioning error sensitivity analysis according to claim 1, characterized in that, the generation of a series of sampling pose points according to the feasible area specifically refers to the attitude of the lidar in actual use restricting the random sampling range of the lidar roll and pitch directions; At different attitudes of the lidar, obtain multiple simulated sampling lidar scans at different attitudes of a single sampling position; Then, plan a feasible area in the lidar point cloud map and divide the feasible area into different sampling grids; The lidar will sample at the grid center with different attitudes.

4. The map compression method based on positioning error sensitivity analysis according to claim 1, characterized in that, the positioning error sensitivity matrix is divided into a line feature point positioning error sensitivity matrix and a surface feature point positioning error sensitivity matrix; The positioning error sensitivity matrix J of line feature points l The calculation formula is as follows: where n lj is the normal vector of the line segment l lj corresponding to the line feature point p, is the skew-symmetric matrix of n lj , the superscript T represents the matrix transpose, and I 3 is the 3×3 identity matrix; The positioning error sensitivity J of surface feature points s The calculation formula is as follows: where n sj is the normal vector of the plane s corresponding to the surface feature point p sj , and the superscript T represents the matrix transpose.

5. The map compression method based on positioning error sensitivity analysis according to claim 1, characterized in that, In each compression unit, the contribution degree of map points is screened. The original map is divided into grids of uniform size, called compression units, and the compression of map points is carried out in each unit. When the number of map points in a compression unit is too small to provide enough map points to meet the given compression ratio, this compression unit is merged with adjacent compression units until the required number of map points is reached.

6. A map compression method based on positioning error sensitivity analysis according to claim 1, characterized in that the high-contribution map points are retained in an adaptive manner. First, when the high-contribution map points in a compression unit are too concentrated, random sampling is performed in this compression unit to make the distribution of the compressed map points more uniform. Then, to increase the compressibility of the point cloud map, while ensuring that the compression ratio of the overall map points remains unchanged, the proportion of the number of line feature points and surface feature points in the compressed map is appropriately increased.

Images