A kind of map constructing method based on inertial navigation unit Yu laser radarTechnical field
The present invention relates to a kind of map constructing method, be specifically related to a kind of combine inertial navigation unit and laser radarIndoor plane map constructing method.
Background technology
Indoor map based on laser radar builds the most extensively to be sent out in robot field's application.Common are 3d laser thunderReach and 2d laser radar.The 3d model of the whole environment of 3d Laser Radar Scanning, 2d laser radar as robot bottom, surface sweeping put downFace base map.Above two method is in accordance with key frame splicing and realizes base map structure.This base map construction method is due to rightThe location estimation of sensor is the change in displacement obtained by the splicing of adjacent two pins, and when indoor large area scanning, two is biphaseThe integration that ortho position is moved can produce cumulative error so that final base map builds poor effect.Especially for laser radar rangeThe biggest situation, the error of this situation pose accumulation can be bigger.Other large area situations there will be the distortion of map entiretySituation.
Summary of the invention
Traditional indoor map construction method based on 2d laser radar is combined with inertial navigation and improves by the present inventionThe precision of map structuring, reduces the distortion sense of map.Particular content is as follows:
A kind of map constructing method based on inertial navigation unit Yu laser radar, comprises the following steps:
1) scanning platform is built
Scanning platform is made up of the robot with two-wheel mobile platform, and platform carries a laser radar and inertial navigation listUnit；
2) scan data
Remote control two-wheel mobile platform, is scanned with laser radar, obtains scan data, the inertial navigation simultaneously carried from platformThe posture information of unit reading platform；
3) interframe splicing
Two adjacent frames are taked ICP closest approach alternative manner to obtain the pose of platform by the data gone out for Laser Radar ScanningChange；
4) closed loop detection
Extract the characteristic point of all scanning frames and characteristic point is stored in characteristics dictionary；To each new frame, look in characteristics dictionaryLook for coupling, if the match is successful, complete closed loop detection；
5) figure optimization
Between the series of frames obtaining step 3), the pose of splicing carries out figure optimization, with the pose of each frame observation moment is whereinNode, the pose of adjacent two frames is changed to limit and builds optimization figure；Optimization figure input isam optimization method storehouse is calculated, is optimizedThe central point pose of each frame gone out, and include the optimization figure after the process of central point pose；
6) inertial navigation data is added
In optimization figure after the treatment, the inertial navigation list read in conjunction with moment of each frame laser radar data collectionThe pose change information of unit, will add the pose change obtained by inertial navigation unit, so constitute between corresponding nodeIf having limit between every two nodes of optimization figure, there are two.
7) quadratic diagram optimization
Optimization figure after above-mentioned process is reruned the central point pose that figure optimization method obtains each frame of optimization；
8) render
Obtain the central point pose of each frame from step 7) after, opengl is used to draw out occupancy grid map, whereinSampled point is labeled as black, and the line from sampled point to center position is plotted as white.
Owing to inertial navigation unit provides pose change substantially so that when laser radar data is carried out ICP splicingThe calculating time is greatly shortened.And inertial navigation unit can measure the inclination of robot, laser when can identify inclination accordinglyThe invalid data of radar, reduces the interference to location.
Detailed description of the invention
A kind of map constructing method based on inertial navigation unit Yu laser radar, comprises the following steps:
1) scanning platform is built
Scanning platform is made up of the robot with two-wheel mobile platform, and platform carries a laser radar and inertial navigation listUnit；
2) scan data
Remote control two-wheel mobile platform, is scanned with laser radar, obtains scan data, the inertial navigation simultaneously carried from platformThe posture information of unit reading platform；
3) interframe splicing
Two adjacent frames are taked ICP closest approach alternative manner to obtain the pose of platform by the data gone out for Laser Radar ScanningChange；
4) closed loop detection
Extract the characteristic point of all scanning frames and characteristic point is stored in characteristics dictionary；To each new frame, look in characteristics dictionaryLook for coupling, if the match is successful, complete closed loop detection；
5) figure optimization
Between the series of frames obtaining step 3), the pose of splicing carries out figure optimization, with the pose of each frame observation moment is whereinNode, the pose of adjacent two frames is changed to limit and builds optimization figure；Optimization figure input isam optimization method storehouse is calculated, is optimizedThe central point pose of each frame gone out, and include the optimization figure after the process of central point pose；
6) inertial navigation data is added
In optimization figure after the treatment, the inertial navigation list read in conjunction with moment of each frame laser radar data collectionThe pose change information of unit, will add the pose change obtained by inertial navigation unit, so constitute between corresponding nodeIf having limit between every two nodes of optimization figure, there are two.
7) quadratic diagram optimization
Optimization figure after above-mentioned process is reruned the central point pose that figure optimization method obtains each frame of optimization.
8) render
Obtain the central point pose of each frame from step 7) after, opengl is used to draw out occupancy grid map, whereinSampled point is labeled as black, and the line from sampled point to center position is plotted as white.