AMCL positioning method based on point and voxel matching measurement model

Abstract

The invention discloses an AMCL positioning method based on a point and voxel matching measurement model, and the method comprises the following steps: S1, reading a point cloud map, carrying out the voxelization processing of the point cloud map, obtaining the index of each voxel, and obtaining a mean vector and an information matrix of a point cloud in each voxel; s2, for the randomly generated particles, obtaining the initial pose of each particle; S3, updating the pose of each particle, and obtaining the predicted pose of each particle at the current moment; and S4, acquiring a newly input point cloud, traversing all particles, transforming the original coordinate system of the newly input point cloud into a map coordinate system according to the predicted pose of each particle, acquiring an index of a voxel corresponding to the point, and acquiring a matching score of the point and the corresponding voxel according to the mean vector and the information matrix of the point cloud in the voxel. According to the improved AMCL positioning method based on voxel matching, the matching accuracy is remarkably improved, meanwhile, the memory occupation is effectively reduced, and the calculation efficiency is improved.

Description

Technical Field

[0001] The invention relates to the technical field of robot positioning, in particular to an AMCL positioning method based on a point and voxel matching measurement model. Background Art

[0002] When positioning a robot equipped with a laser radar, it usually integrates the data of sensors such as IMU (inertial measurement unit), laser radar, and wheel speed meter with the pre-built point cloud map for positioning. The current relatively mature and widely used open source algorithm for robot positioning is AMCL (Adaptive Monte Carlo). The existing laser point cloud AMCL positioning method specifically includes the following steps:

[0003] S1: Initialization: Read the point cloud map cloud map , construct kdtree; initialize pose;

[0004] S2: Randomly generate N particles (usually 80), each particle p i (i∈[0, N-1]) contains the 6-dimensional pose χ(x, y, z, roll, pitch, yaw), where x, y, z are Cartesian coordinates in the map coordinate system, and roll, pitch, yaw are Euler angles;

[0005] S3: Prediction: Based on the odometer data input, calculate the position and posture of each particle at time t-1 t-1 Pose prediction at time t The superscript * indicates the predicted value;

[0006] S4: Measurement: When a new point cloud is input, all particles are traversed and the position of each particle is measured. Input point cloud lidar (The subscript lidar represents the laser radar coordinate system) is transformed to the map coordinate system cloud map (The subscript map indicates the map coordinate system); traverse the cloud map All points p i , use kdtree to search for point p i The nearest neighbor p in the map point cloud nn , calculate p i With p nn The Euclidean distance dis i ; The matching score is calculated as follows:

[0007] dist = match_dis min -max(dis i , match_dis flat )

[0008] scorei =dist*match weight , if dist>0: num++

[0009]

[0010] where match_dis min is the matching distance threshold (unit: m, usually 0.2), match_dis flat is the expansion parameter (unit: m, usually 0.05), match weight is the distance residual coefficient (usually 5.0); here all points with dist>0 are considered to be matched successfully, num is the number of points that are matched successfully, and participates in the calculation of the total score. The score is the sum of the scores of all the successfully matched points. i Find the average value;

[0011] S5: Resampling: Sample and obtain M particles (M and N may be different) from all particles according to the scores / weights calculated in step S4.

[0012] S6: Repeat steps S3 to S5 to update the current position of the robot in real time.

[0013] Although the AMCL algorithm has high computational efficiency and good adaptability, the measurement model uses a beam model (beam_range_finder_model) or a likelihood domain model (likelihood_field_range_finder_model) to calculate the matching effect between the point cloud and the map. For example, the aforementioned AMCL positioning method uses a likelihood domain model to calculate the matching between the point cloud and the map. Since the wave velocity model has a large amount of calculation, is troublesome to adjust the parameters based on four artificial assumptions, and has an uneven probability distribution, it is relatively less used; the likelihood domain model is an improvement based on the beam model, with a smooth probability distribution and high computational efficiency, but due to the use of point-to-point matching, it is easy to mismatch and has poor accuracy. In addition, the method of searching the nearest neighbor uses kdtree, which has a high memory usage and slow search when the number of points in the point cloud map is large.

[0014] Therefore, in order to realize the point-to-voxel matching method, the present invention improves the existing AMCL positioning to solve the above technical defects. Summary of the invention

[0015] The purpose of the present invention is to overcome the above-mentioned deficiencies in the prior art and to provide an AMCL positioning method based on a point and voxel matching measurement model.

[0016] The technical solution of the present invention is: an AMCL positioning method based on a point and voxel matching measurement model, comprising the following steps:

[0017] S1: Read the point cloud map, voxelize the point cloud map, obtain the index of each voxel, and obtain the mean vector and information matrix of the point cloud in each voxel;

[0018] S2: For randomly generated particles, get the initial position of each particle

[0019] S3: Update the position and posture of each particle to obtain the predicted position and posture of each particle at the current moment;

[0020] S4: Get the newly input point cloud, traverse all particles, transform the original coordinate system of the newly input point cloud to the map coordinate system according to the predicted position of each particle, get the index of the voxel corresponding to the point, and get the matching score between the point and the corresponding voxel according to the mean vector and information matrix of the point cloud in the voxel;

[0021] S5: resampling according to the matching score to generate a new particle set;

[0022] S6: Repeat steps S3 to S5 to update the current position of the navigation object in real time.

[0023] Furthermore, in step S1, the point cloud of the point cloud map is divided into voxels of size res, and the index voxel corresponding to each point in the point cloud is calculated. idx :

[0024]

[0025] In the formula, floor() is the rounding function, (p x ,p y ,p z ) is the coordinate of a point in the point cloud, and res is the voxel resolution parameter.

[0026] Further, in step S1, the mean vector mean of the point cloud within the voxel i and the information matrix inf o i Obtained by the following formula:

[0027]

[0028] In the formula, Represents the three-dimensional vector of a point; j represents the jth point in the voxel; num is the number of points in the voxel; Cov i is the covariance matrix of the point cloud within the voxel, also known as the information matrix inf o i The inverse of , T represents the transpose of the matrix.

[0029] Further, in step S4, the point p is calculated according to the coordinate value of the point in the map coordinate system and the voxel resolution parameter res. i The corresponding voxel index

[0030]

[0031] Furthermore, suppose that point p i Matching to the jth voxel, the corresponding voxel mean j , information matrix inf o j , calculate point p by the following formula i The matching score with the voxel i :

[0032] redidual i =[(p x , p y , p z )-mean j ] T * inf o j *[(p x , p y , p z )-mean j ]

[0033] If redidual i >th residual :num++

[0034]

[0035] In the formula, * represents matrix multiplication, T represents the transpose of the matrix, and residual i For point p i The residual error matched to the voxel, th residual is the chi-square detection threshold; num is the number of successfully matched points; match score score i The score is obtained by the e exponential function. i Obey Gaussian fractions.

[0036] Furthermore, if residual i >th residual , the match is considered a failure and the points for successful matching will not be increased; only when residual i ≤th residual , the match is considered successful, and the num++ operation will be executed, and the number of successfully matched points num will increase by 1.

[0037] Further, according to point p i The matching score with the voxel i , the mean score of the matching scores of all points in the point cloud and their corresponding voxels is obtained by the following formula:

[0038]

[0039] Further, in step S4, a newly input point cloud is obtained, which is located in the original coordinate system of the sensor; and according to the position and posture information of the sensor, the point cloud is converted from the sensor coordinate system to the map coordinate system.

[0040] Further, in step S4, the voxel resolution parameter res=1.

[0041] Beneficial effects of the present invention:

[0042] (1) By dividing the point cloud data into voxels, each voxel contains a mean vector and an information matrix. This method can significantly reduce memory usage, especially when the resolution is high. The memory usage of the voxel map is significantly reduced compared to the original point cloud map, saving storage space and improving processing efficiency. In addition, the mean vector in each voxel can represent the equilibrium position of the points in the area, while the information matrix can represent the distribution characteristics of the points in the area, thereby more accurately capturing the characteristics of the local area, while reducing the redundancy of the point cloud data and improving the processability and accuracy of the data.

[0043] (2) By traversing all particles, the original coordinate system of the input point cloud is transformed into the map coordinate system according to the predicted position of each particle, and the matching score between the point and the voxel is obtained. Compared with the traditional kdtree nearest neighbor search method, the computational complexity and time complexity of the point cloud matching process are reduced. Due to the significant improvement in search efficiency and the reduction in computational complexity, the system can complete the matching and updating of point cloud data in a shorter time, thereby greatly improving the response speed and processing capability of the system.

[0044] (3) Using the mean vector and information matrix within the voxel to obtain the matching score between the point and the voxel, compared with the traditional method of matching only through distance calculation, it can take into account the local structure of the space and the characteristics of data distribution, and can significantly improve the accuracy and efficiency of matching, especially in complex scenes or in the presence of noise; that is: the mean vector helps to more accurately describe the morphology of the local area by summarizing the geometric features of the points within the voxel; the information matrix provides the covariance information of the point cloud data, which enhances the robustness to noise and uncertainty in the matching process. In this way, when processing large-scale point cloud data, not only can the computational efficiency be improved, but also the spatial consistency of the matching results can be ensured;

[0045] (4) The present invention combines point-to-voxel matching with AMCL positioning, which can effectively overcome the shortcomings of AMCL positioning, such as large memory usage, large amount of calculation and low matching accuracy, while retaining the advantages of AMCL, such as high robustness, global positioning capability, self-correction capability and parallel processing capability; and point-to-voxel matching optimizes the positioning accuracy of AMCL by providing accurate local environmental information, while the particle filtering method of AMCL enhances the system's adaptability to dynamic environments and the stability of positioning. The combination of the two not only improves the computational efficiency, but also enhances the positioning accuracy and robustness, making the system perform better in complex environments. DETAILED DESCRIPTION

[0046] The present invention is further described in detail below through specific examples.

[0047] An AMCL positioning method based on a point and voxel matching measurement model specifically comprises the following steps:

[0048] S101: Read the point cloud map, perform voxel processing on the point cloud map, obtain the index of each voxel, and obtain the mean vector and information matrix of the point cloud data in each voxel.

[0049] Specifically, the point cloud of the point cloud map is divided into voxels of size res (voxel resolution parameter, usually 1, unit m), and the index voxel corresponding to each point in the point cloud is calculated. idx :

[0050]

[0051] Where floor( ) is a floor rounding function, for example, floor(1.1) = 1; (p x ,p y ,p z ) is the coordinate value of a point in the point cloud corresponding to the three axes x, y, and z of the Cartesian coordinates in the laser radar coordinate system, and res is the size of the voxel.

[0052] After traversing all points, all points in the point cloud map correspond to each voxel, and each voxel contains multiple points cloud_voxel i and a voxel index voxel idxSpecifically, after traversing all the points in the point cloud map and calculating the voxel index corresponding to each point, a mapping relationship will be formed, that is, all the points in the point cloud map are assigned to each voxel. After the voxel is divided, a certain number of points will be gathered in each voxel. For example, voxel A may contain point 1, point 2, point 3, etc. in the point cloud. After the voxel division calculation, the coordinates of these points fall within the range of voxel A, so they are all included in voxel A. These points are represented as cloud_voxel i Each voxel has a unique index identifier, which is used to distinguish different voxels. The voxel index is represented by voxel idx .

[0053] Calculate all points in each voxel cloud_voxel i The mean vector mean i and the information matrix inf o i :

[0054]

[0055] In the formula, Represents the three-dimensional vector of a point; j represents the jth point in the voxel; num is the number of points in the voxel; Cov i The point cloud in the voxel is cloud_voxel i The covariance matrix of is also the information matrix inf o i The inverse of , T represents the transpose of the matrix.

[0056] Point cloud map map Each point in the voxel map is represented by (x, y, z) coordinates. map After the representation method, each voxel is represented by the mean vector mean x ,mean y ,mean z And the information matrix inf o0…inf o9 is represented. Since the number of voxels is much smaller than the number of point clouds in the point cloud map, for the case of res=1, the memory usage of the voxel map is about 1 / 4 smaller than that of the point cloud map. For a larger res, the memory usage will be relatively smaller.

[0057] Steps S102 and S103 are the same as the original AMCL steps in the background technology, namely:

[0058] S102: Randomly generate N particles (usually 80), each particle contains a 6-dimensional pose χ(x, y, z, roll, pitch, yaw), where x, y, z are Cartesian coordinates in the map coordinate system, and roll, pitch, yaw are Euler angles. Each particle represents a possible position and pose of the robot in the environment.

[0059] S103: Prediction: Predict the state of each particle at the current moment based on odometer data (such as IMU, wheel odometer, etc.), that is, calculate the position and posture x of each particle based on time t-1 t-1 Predicted position at time t The superscript * indicates the predicted value.

[0060] S104: Measurement: When a new point cloud is input, all particles are traversed, and according to the predicted position of each particle, the lidar coordinate system of the input point cloud is transformed into the map coordinate system, the index of the point and the corresponding voxel is obtained, and the matching score of the point and the corresponding voxel is obtained according to the mean vector and information matrix of the point cloud data in the voxel.

[0061] Specifically, when a new point cloud is input, all particles are traversed and the predicted pose of each particle is calculated. The laser radar coordinate system cloud of the input point cloud lidar Transform to map coordinate system cloud map Next; traverse the cloud lidar All points p i , because here p i The coordinates of the point have been transformed to the map coordinate system, so p can be directly calculated based on the coordinate value of the point in the map coordinate system and the voxel resolution parameter res i The corresponding voxel index

[0062]

[0063] Compared with the kdtree method of searching for the nearest neighbor, the time complexity is reduced from O(log(n)) to O(1), where n is the point cloud map cloud map The number of points greatly improves the search efficiency. Get the voxel index That is to say, we get p i With voxel maps map The voxel correspondence in .

[0064] Assume point p i Matching to the jth voxel, the corresponding voxel mean j , information matrix inf o j , calculate point p by the following formula iThe matching score with the voxel i , and get the mean score of the matching scores of all points in the point cloud and their corresponding voxels:

[0065] residual i =[(p x , p y , p z )-mean j ] T * inf o j *[(p x , p y , p z )-mean j ]

[0066] if residual i >th residual :num++

[0067] In the formula, * represents matrix multiplication, T represents the transpose of the matrix, and residual i For point p i The residual error matched to the voxel, th residual is the chi-square detection threshold (usually 6.25, which can be obtained by looking up the chi-square table); num is the number of points that are successfully matched. i >th residual , the match is considered to have failed, and the num++ operation will not be executed, that is, the number of successful matches will not be increased; only when residual i ≤th residual The match is considered successful only when num++ is executed, and the number of successful matching points num increases by 1. In addition, the matching score score i The score is obtained by the e exponential function. i Obey Gaussian fractions.

[0068] Since point-to-voxel matching is adopted, using point p i The matching score is calculated by using the statistical information mean and info of the point cloud within the voxel i , so score i It is a good example of input point p i Whether the distribution of point clouds within voxels is satisfied. For example, if the positioning is correct and the map has not changed, the point cloud distribution obtained by scanning the same object at any time should be consistent. Compared with the original AMCL method of matching points with multiple points within a voxel, this embodiment uses a method of matching points with multiple points within a voxel, which can reduce mismatches and improve the accuracy and stability of the algorithm.

[0069] S105: Resampling: Sample and obtain M particles from all particles according to the matching scores calculated in step S104 (M may be different from N in step S102).

[0070] S106: Repeat steps S103 to S105 to update the current position and posture of the robot in real time.

[0071] This is an improved AMCL positioning method based on voxel matching, which significantly improves the matching accuracy, while effectively reducing memory usage and improving computational efficiency.

Claims

1. An AMCL positioning method based on a point and voxel matching measurement model, characterized in that: The following steps are involved: S1: Read the point cloud map, voxelize the point cloud map, obtain the index of each voxel, and obtain the mean vector and information matrix of the point cloud in each voxel; S2: For randomly generated particles, get the initial position of each particle S3: Update the position and posture of each particle to obtain the predicted position and posture of each particle at the current moment; S4: Get the newly input point cloud, traverse all particles, transform the original coordinate system of the newly input point cloud to the map coordinate system according to the predicted position of each particle, get the index of the voxel corresponding to the point, and get the matching score between the point and the corresponding voxel according to the mean vector and information matrix of the point cloud in the voxel; S5: resampling according to the matching score to generate a new particle set; S6: Repeat steps S3 to S5 to update the current position of the navigation object in real time.

2. The AMCL positioning method based on the point and voxel matching measurement model according to claim 1, characterized in that: In step S1, the point cloud of the point cloud map is divided into voxels of size res, and the index voxel corresponding to each point in the point cloud is calculated. idx : In the formula, floor( ) is the rounding function, (p x , p y , p z ) is the coordinate of a point in the point cloud, and res is the voxel resolution parameter.

3. The AMCL positioning method based on the point and voxel matching measurement model according to claim 1, characterized in that: In step S1, the mean vector mean of the point cloud in the voxel i and information matrix info i Obtained by the following formula: In the formula, Represents the three-dimensional vector of a point; j represents the th point in the voxel; num is the number of points in the voxel; Cov i is the covariance matrix of the point cloud within the voxel, also known as the information matrix info i The inverse of , T represents the transpose of the matrix.

4. The AMCL positioning method based on the point and voxel matching measurement model according to claim 1, characterized in that: In step S4, the point p is calculated based on the coordinate value of the point in the map coordinate system and the voxel resolution parameter res. i The corresponding voxel index 5. The AMCL positioning method based on the point and voxel matching measurement model according to claim 1, characterized in that: In step S4, assume that point p i Matching to the jth voxel, the corresponding voxel mean j , information matrix info j , calculate point p by the following formula i The matching score with the voxel i : residual i =[(p x ,p y ,p z )-mean j ] T *info j *[(p x ,p y ,p z )-mean j ] if residual i >th residual :num++ In the formula, * represents matrix multiplication, T represents the transpose of the matrix, and residual i For point p i The residual error matched to the voxel, th residual is the chi-square detection threshold; num is the number of successfully matched points; match score score i The score is obtained by the e exponential function. i Obey Gaussian fractions.

6. The AMCL positioning method based on the point and voxel matching measurement model according to claim 5, characterized in that: If residual i >th residual , the match is considered a failure and the points for successful matching will not be increased; only when residual i ≤th residual , the match is considered successful, and the num++ operation will be executed, and the number of successfully matched points num will increase by 1.

7. The AMCL positioning method based on the point and voxel matching measurement model according to claim 5 or 6, characterized in that: According to point p i The matching score with the voxel i , the mean score of the matching scores of all points in the point cloud and their corresponding voxels is obtained by the following formula:

8. The AMCL positioning method based on the point and voxel matching measurement model according to claim 1, characterized in that: In step S4, a newly input point cloud is obtained, which is located in the original coordinate system of the sensor; and the point cloud is converted from the sensor coordinate system to the map coordinate system according to the position and posture information of the sensor.

9. The AMCL positioning method based on the point and voxel matching measurement model according to claim 2, characterized in that: In step S4, the voxel resolution parameter res=1.

Images