Background
As mobile robots are applied more and more widely in the fields of service and warehouse logistics, the autonomous positioning navigation technology is more and more important. The technologies mainly applied at present are magnetic navigation, two-dimensional code navigation and laser navigation, wherein the magnetic navigation is suitable for the environment with a relatively fixed working track, and an electromagnetic wire or a magnetic strip needs to be laid on a path. Although the two-dimensional code navigation has a free track, a large number of two-dimensional codes need to be pasted on the path, and regular maintenance is needed. The laser navigation positioning is accurate, the path is flexible and changeable, other positioning facilities are not needed, and the method is suitable for various field environments, so that the method becomes a mainstream positioning navigation mode of the mobile robot.
In positioning and navigation applications based on 2d laser radar, the motion amount of a mobile robot is generally calculated by using adjacent frame point cloud registration to obtain the pose of the robot or assist the robot in positioning. The existing 2d laser radar point cloud registration scheme is mainly an NDT point cloud registration method, wherein a reference frame point cloud is rasterized, a normal distribution PDF parameter is calculated, and a least square optimization problem is established through probability score of a target point cloud on a grid.
Firstly, dividing the space occupied by reference frame point cloud into grids or voxels with specified sizes, and calculating the multi-dimensional normal distribution parameter PDF of each grid; secondly, calculating a probability distribution model of the grid, and calculating the center (each axis mean value) of a contained point in the grid and a covariance matrix; then, projecting the point cloud to be registered under a reference coordinate system (the coordinate system of the reference point cloud is usually the previous frame), calculating the probability of each conversion point falling in the corresponding grid according to the normal distribution parameters, and calculating the sum of the probabilities of the corresponding point cloud falling in the corresponding grid; and finally, optimizing the objective function according to a Newton optimization algorithm, namely searching a transformation parameter to enable the value of the final score to be maximum, and repeating iteration until convergence. This solution has two disadvantages:
1) for the structured point cloud, the NDT does not assume the Gaussian distribution of a grid, and the effect is not good;
2) the accuracy of rigid body transformation between two frames of point clouds obtained by calculation is not enough.
Disclosure of Invention
The invention provides a grid-occupied-based 2D laser sequence point cloud registration method, aiming at improving the problems.
The invention is realized in such a way that a 2D laser sequence point cloud registration method based on an occupied grid specifically comprises the following steps:
s1 points based on two adjacent frames of laser pointst-1And pointstGenerating corresponding grid map girdmapt-1And girdmapt;
S2, traversing all candidate poses in the whole search window according to the displacement step length and the angle step length, and projecting the laser frame point cloud at the moment t to a girdmap of a grid map based on the candidate posest-1The candidate pose T (theta, T) with the highest scorex,ty) As an initial pose;
s3, generating a transformation matrix T '(theta', T ') from the point cloud at the T-1 moment to the point cloud at the T moment based on the initial pose'x,t′y) Based on a transformation matrix T ' (θ ', T 'x,t′y) The laser frame point clouds points at the time ttOccupancy grid map girdmap projected to time t-1t-1In the method, the laser frame point clouds points at the time of t-1 are addedt-1Occupation grid map girdmap projected to time ttPerforming the following steps;
s4, points of sequence pointtAnd pointst-1All the laser points construct an objective function, and the minimum of the objective function is determinedAnd taking the corresponding cost pose as the pose at the current time t.
Further, the objective function is specifically as follows:
Si(xi) representing laser point clouds pointst-1And pointstTransforming the ith laser point to the grid map coordinate system, M (S)i(ξ)) represents the probability that the ith laser point is occupied at the location on the corresponding grid map.
Further, before step S1, the method further includes:
point cloud filtering: and removing outliers in the point cloud by using a statistical probability filter, and reducing the density of the point cloud on the basis of ensuring the microscopic characteristics of the point cloud by using a voxel filter.
The grid-occupied-based 2D laser sequence point cloud registration method has the following beneficial effects:
1) the requirements on the number and the quality of point clouds are low, the computing speed is high, and the matching precision is high;
2) the rough pose of the registration of the adjacent point clouds can be quickly found through correlation matching, the calculated amount is small, the calculating speed is high, and an initial value is provided for subsequent fine matching. The nonlinear optimization matching can obtain a high-precision matching result through continuous iteration through a unified matching cost function. Compared with an ICP (inductively coupled plasma) matching algorithm, the method can obtain a better matching result without providing an initial value, the matching precision of the algorithm is higher compared with an NDT (normalized data aided test) matching algorithm, and compared with the two algorithms, the algorithm has the advantages of lower required point cloud number and higher adaptability to different devices under the condition of the same precision.
Detailed Description
The following detailed description of the embodiments of the present invention will be given in order to provide those skilled in the art with a more complete, accurate and thorough understanding of the inventive concept and technical solutions of the present invention.
The invention aims to solve the problem of how to efficiently and accurately calculate the motion amount of the mobile robot by using the sequence 2d laser point cloud in the positioning and navigation process of the mobile robot, thereby realizing or assisting the mobile robot to calculate the position of the mobile robot. The sequence point cloud refers to laser point cloud scanned by a radar in sequence, and the method aims at adjacent frame matching of the sequence point cloud. Knowing the points of neighboring pointst-1And pointstWherein pointst-1Points of the laser frame at time t-1tThe laser frame point cloud is the time t, wherein the time t is the current time, and the time t-1 is the last time.
Fig. 1 is a flowchart of a 2D laser sequence point cloud registration method based on an occupancy grid according to an embodiment of the present invention, where the method specifically includes the following steps:
step 1) point cloud filtering: the original laser point cloud has some obvious outliers, which easily causes mismatching, so that the outliers in the point cloud are removed by using a statistical probability filter. In addition, the radar sampling rate is generally very high, thousands of points exist in one frame of laser point cloud, the calculation time is consumed in the point cloud matching process, the voxel filter is used, the density of the point cloud is reduced on the basis of ensuring the microscopic characteristics of the point cloud, and the matching efficiency is improved.
Step 2) generating a grid probability map: let the position of the laser point cloud be (0,0,0), and for each scanning beam, the starting point of one beam is known
And an end point
k is the number of grids the beam passes through, and the probability of the beam passing through the grids can be updated by using the log-of-superiority integration method. To pair
The occupied grid is updated, and the slave is updated
To
The grids passed by are freely updated. The freely updated value oddsFree is set to 0.4 and the updated value oddsoc is taken to 0.6. Given that the occupation probability of the current grid is p, the odds value of the current grid is odds ═ log (p/(1-p)), if the current grid is updated freely, odds is made odds + oddsFree, if the current grid is updated, odds is made odds + oddsoc, and the occupation probability of the updated grid is p ═ exp (odds))/(exp (odds)) + 1). Respectively using two adjacent frames of laser point clouds points
t-1And points
tGenerating corresponding grid map girdmap
t-1And girdmap
t。
Step 3) correlation scan matching
In order to avoid the situation that the fine matching based on the optimization falls into a local extreme value and causes a wrong matching result, an initial value needs to be calculated by using the correlation scanning matching, and the matching process is as follows:
a) generating a search window with a specified size by taking the pose at the time t-1 as a center, and calculating a linear _ search _ window of a displacement search window v/q and an angular search window angularjsearch _ window ω/q according to the maximum linear velocity v, the maximum angular velocity ω and the laser radar frequency q of the robot;
b) calculating a search step length, wherein the displacement search step length is a grid, and an angle search step length calculation formula is as follows:
angular_step=acos(1-r2/(2,max_d2))
wherein r is the resolution of the grid map, and max _ d is the farthest point distance of the current point cloud;
c) traversing all candidate poses in the whole search window according to the displacement step length and the angle step length, projecting the laser frame point cloud at the T moment into a grid map under each candidate pose, accumulating and calculating the occupation probability of a projection grid to obtain the score of the current candidate pose, and calculating the candidate pose T (theta, T) with the highest scorex,ty) And (5) taking the pose as an initial pose to enter the next calculation.
Step 4) matching based on optimization:
obtaining filtered adjacent laser frame point clouds points in the step 1)t-1And pointstStep 2) obtaining a girdmap of the occupied grid map generated by the adjacent frames of laser point cloudst-1And girdmaptCalculating to obtain the transformation T (theta, T) from the point cloud at the T-1 moment to the point cloud at the T moment in the step 3)x,ty) A transformation matrix T '(θ', T ') from the point cloud at time T to the point cloud at time T-1 can be calculated'x,t′y) Wherein
a) Based on a transformation matrix T ' (θ ', T 'x,t′y) Point clouds points at t momenttOccupancy grid map girdmap projected to time t-1t-1In the method, the point clouds points at the time of t-1 are putt-1Occupation grid map girdmap projected to time ttPerforming the following steps;
b) for sequence point clouds pointstAnd pointst-1All laser spots in (1) construct the following objective function:
Si(xi) representing laser point clouds pointst-1And pointstTransforming the ith laser point to the grid map coordinate system, M (S)i(ξ)) represents the probability that the ith laser point is occupied at the position on the corresponding grid map, thus transforming the pose calculation into a least squares solution problem;
c) calculating a score M (S) for each point using the bicubic differencei(xi)), and for M (S)i(xi)) calculating partial derivatives, converting the least square solution of pose calculation into an iterative pose incremental calculation problem, and taking the pose with the lowest least square cost as the position of the current time tPosture.
The grid-occupied-based 2D laser sequence point cloud registration method has the following beneficial effects:
1) the requirements on the number and the quality of point clouds are low, the computing speed is high, and the matching precision is high;
2) the rough pose of the registration of the adjacent point clouds can be quickly found through correlation matching, the calculated amount is small, the calculating speed is high, and an initial value is provided for subsequent fine matching. The nonlinear optimization matching can obtain a high-precision matching result through continuous iteration through a unified matching cost function. Compared with an ICP (inductively coupled plasma) matching algorithm, the method can obtain a better matching result without providing an initial value, the matching precision of the algorithm is higher compared with an NDT (normalized data aided test) matching algorithm, and compared with the two algorithms, the algorithm has the advantages of lower required point cloud number and higher adaptability to different devices under the condition of the same precision.
The invention is not limited by the above-mentioned manner, and various insubstantial modifications of the inventive concept and solution are possible, or the inventive concept and solution can be directly applied to other applications without modification.